From f01c06f3043a01b4d578dbd76d1eb1f1d60fb384 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 18 Mar 2022 15:13:13 -0300 Subject: [PATCH 1/9] Add Interval class (#388) Signed-off-by: Michel Hidalgo --- examples/CMakeLists.txt | 18 ++ examples/interval_example.cc | 60 ++++++ include/ignition/math/Interval.hh | 292 ++++++++++++++++++++++++++++++ src/Interval_TEST.cc | 222 +++++++++++++++++++++++ 4 files changed, 592 insertions(+) create mode 100644 examples/interval_example.cc create mode 100644 include/ignition/math/Interval.hh create mode 100644 src/Interval_TEST.cc diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index ade7a10d5..beb938796 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -18,6 +18,24 @@ target_link_libraries(gauss_markov_process ignition-math${IGN_MATH_VER}::ignitio add_executable(vector2_example vector2_example.cc) target_link_libraries(vector2_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) +add_executable(graph_example graph_example.cc) +target_link_libraries(graph_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + +add_executable(interval_example interval_example.cc) +target_link_libraries(interval_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + +add_executable(kmeans kmeans.cc) +target_link_libraries(kmeans ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + +add_executable(quaternion_from_euler quaternion_from_euler.cc) +target_link_libraries(quaternion_from_euler ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + +add_executable(quaternion_to_euler quaternion_to_euler.cc) +target_link_libraries(quaternion_to_euler ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + +add_executable(rand_example rand_example.cc) +target_link_libraries(rand_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + add_executable(triangle_example triangle_example.cc) target_link_libraries(triangle_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) diff --git a/examples/interval_example.cc b/examples/interval_example.cc new file mode 100644 index 000000000..68623ca72 --- /dev/null +++ b/examples/interval_example.cc @@ -0,0 +1,60 @@ +/* + * 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. + * +*/ +//! [complete] +#include +#include + +int main(int argc, char **argv) +{ + std::cout << std::boolalpha; + + const ignition::math::Intervald defaultInterval; + // A default constructed interval should be empty. + std::cout << "The " << defaultInterval << " interval is empty: " + << defaultInterval.Empty() << std::endl; + + const ignition::math::Intervald openInterval = + ignition::math::Intervald::Open(-1., 1.); + // An open interval should exclude its endpoints. + std::cout << "The " << openInterval << " interval contains its endpoints: " + << (openInterval.Contains(openInterval.LeftValue()) || + openInterval.Contains(openInterval.RightValue())) + << std::endl; + + const ignition::math::Intervald closedInterval = + ignition::math::Intervald::Closed(0., 1.); + + // A closed interval should include its endpoints. + std::cout << "The " << closedInterval << " interval contains its endpoints: " + << (closedInterval.Contains(closedInterval.LeftValue()) || + closedInterval.Contains(closedInterval.RightValue())) + << std::endl; + + // Closed and open intervals may intersect. + std::cout << "Intervals " << closedInterval << " and " << openInterval + << " intersect: " << closedInterval.Intersects(openInterval) + << std::endl; + + // The unbounded interval should include all non-empty intervals. + std::cout << "The " << ignition::math::Intervald::Unbounded + << " interval contains all previous non-empty intervals: " + << (ignition::math::Intervald::Unbounded.Contains(openInterval) || + ignition::math::Intervald::Unbounded.Contains(closedInterval)) + << std::endl; + +} +//! [complete] diff --git a/include/ignition/math/Interval.hh b/include/ignition/math/Interval.hh new file mode 100644 index 000000000..fb0229b10 --- /dev/null +++ b/include/ignition/math/Interval.hh @@ -0,0 +1,292 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_MATH_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: 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 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 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 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 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 + const 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/src/Interval_TEST.cc b/src/Interval_TEST.cc new file mode 100644 index 000000000..ecd0c8ee1 --- /dev/null +++ b/src/Interval_TEST.cc @@ -0,0 +1,222 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include + +#include "ignition/math/Interval.hh" + +using namespace ignition; + +///////////////////////////////////////////////// +TEST(IntervalTest, DefaultConstructor) +{ + const math::Intervald interval; + EXPECT_DOUBLE_EQ( + interval.LeftValue(), + interval.RightValue()); +} + +///////////////////////////////////////////////// +TEST(IntervalTest, Constructor) +{ + constexpr bool kClosed = true; + + const math::Intervald interval( + 0., kClosed, 1., !kClosed); + EXPECT_DOUBLE_EQ(interval.LeftValue(), 0.); + EXPECT_TRUE(interval.IsLeftClosed()); + EXPECT_DOUBLE_EQ(interval.RightValue(), 1.); + EXPECT_FALSE(interval.IsRightClosed()); +} + +///////////////////////////////////////////////// +TEST(IntervalTest, ConstructionHelpers) +{ + const math::Intervald openInterval = + math::Intervald::Open(0., 1.); + EXPECT_DOUBLE_EQ(openInterval.LeftValue(), 0.); + EXPECT_FALSE(openInterval.IsLeftClosed()); + EXPECT_DOUBLE_EQ(openInterval.RightValue(), 1.); + EXPECT_FALSE(openInterval.IsRightClosed()); + + const math::Intervald leftClosedInterval = + math::Intervald::LeftClosed(0., 1.); + EXPECT_DOUBLE_EQ(leftClosedInterval.LeftValue(), 0.); + EXPECT_TRUE(leftClosedInterval.IsLeftClosed()); + EXPECT_DOUBLE_EQ(leftClosedInterval.RightValue(), 1.); + EXPECT_FALSE(leftClosedInterval.IsRightClosed()); + + const math::Intervald rightClosedInterval = + math::Intervald::RightClosed(0., 1.); + EXPECT_DOUBLE_EQ(rightClosedInterval.LeftValue(), 0.); + EXPECT_FALSE(rightClosedInterval.IsLeftClosed()); + EXPECT_DOUBLE_EQ(rightClosedInterval.RightValue(), 1.); + EXPECT_TRUE(rightClosedInterval.IsRightClosed()); + + const math::Intervald closedInterval = + math::Intervald::Closed(0., 1.); + EXPECT_DOUBLE_EQ(closedInterval.LeftValue(), 0.); + EXPECT_TRUE(closedInterval.IsLeftClosed()); + EXPECT_DOUBLE_EQ(closedInterval.RightValue(), 1.); + EXPECT_TRUE(closedInterval.IsRightClosed()); +} + +///////////////////////////////////////////////// +TEST(IntervalTest, UnboundedInterval) +{ + EXPECT_FALSE(math::Intervald::Unbounded.Empty()); + EXPECT_FALSE(math::Intervald::Unbounded.IsLeftClosed()); + EXPECT_FALSE(math::Intervald::Unbounded.IsRightClosed()); +} + +///////////////////////////////////////////////// +TEST(IntervalTest, EmptyInterval) +{ + EXPECT_FALSE(math::Intervald::Open(0., 1.).Empty()); + EXPECT_TRUE(math::Intervald::Open(0., 0.).Empty()); + EXPECT_TRUE(math::Intervald::LeftClosed(0., 0.).Empty()); + EXPECT_TRUE(math::Intervald::RightClosed(0., 0.).Empty()); + EXPECT_FALSE(math::Intervald::Closed(0., 0.).Empty()); + EXPECT_TRUE(math::Intervald::Closed(1., 0.).Empty()); +} + +///////////////////////////////////////////////// +TEST(IntervalTest, IntervalMembership) +{ + const math::Intervald emptyInterval = + math::Intervald::Open(0., 0.); + EXPECT_FALSE(emptyInterval.Contains(0.)); + + const math::Intervald openInterval = + math::Intervald::Open(0., 1.); + EXPECT_FALSE(openInterval.Contains(0.)); + EXPECT_TRUE(openInterval.Contains(0.5)); + EXPECT_FALSE(openInterval.Contains(1)); + + const math::Intervald leftClosedInterval = + math::Intervald::LeftClosed(0., 1.); + EXPECT_TRUE(leftClosedInterval.Contains(0.)); + EXPECT_TRUE(leftClosedInterval.Contains(0.5)); + EXPECT_FALSE(leftClosedInterval.Contains(1)); + + const math::Intervald rightClosedInterval = + math::Intervald::RightClosed(0., 1.); + EXPECT_FALSE(rightClosedInterval.Contains(0.)); + EXPECT_TRUE(rightClosedInterval.Contains(0.5)); + EXPECT_TRUE(rightClosedInterval.Contains(1)); + + const math::Intervald closedInterval = + math::Intervald::Closed(0., 1.); + EXPECT_TRUE(closedInterval.Contains(0.)); + EXPECT_TRUE(closedInterval.Contains(0.5)); + EXPECT_TRUE(closedInterval.Contains(1)); + + const math::Intervald degenerateInterval = + math::Intervald::Closed(0., 0.); + EXPECT_TRUE(degenerateInterval.Contains(0.)); +} + +///////////////////////////////////////////////// +TEST(IntervalTest, IntervalSubset) +{ + const math::Intervald openInterval = math::Intervald::Open(0., 1.); + EXPECT_FALSE(openInterval.Contains(math::Intervald::Open(0., 0.))); + EXPECT_FALSE(openInterval.Contains(math::Intervald::Closed(-1., 0.))); + EXPECT_FALSE(openInterval.Contains(math::Intervald::Closed(1., 2.))); + EXPECT_FALSE(openInterval.Contains(math::Intervald::Open(0.5, 1.5))); + EXPECT_FALSE(openInterval.Contains(math::Intervald::Open(-0.5, 0.5))); + EXPECT_TRUE(openInterval.Contains(math::Intervald::Open(0.25, 0.75))); + EXPECT_FALSE(openInterval.Contains(math::Intervald::Closed(0., 1.))); + + const math::Intervald closedInterval = math::Intervald::Closed(0., 1.); + EXPECT_FALSE(closedInterval.Contains(math::Intervald::Open(0., 0.))); + EXPECT_TRUE(closedInterval.Contains(math::Intervald::Closed(0., 0.))); + EXPECT_FALSE(closedInterval.Contains(math::Intervald::Closed(0.5, 1.5))); + EXPECT_FALSE(closedInterval.Contains(math::Intervald::Closed(-0.5, 0.5))); + EXPECT_TRUE(closedInterval.Contains(math::Intervald::Closed(0.25, 0.75))); + EXPECT_TRUE(closedInterval.Contains(math::Intervald::Open(0., 1.))); + + EXPECT_TRUE(math::Intervald::Unbounded.Contains(openInterval)); + EXPECT_TRUE(math::Intervald::Unbounded.Contains(closedInterval)); +} + +///////////////////////////////////////////////// +TEST(IntervalTest, IntervalEquality) +{ + EXPECT_NE(math::Intervald::Open(0., 0.), math::Intervald::Open(0., 0.)); + EXPECT_EQ(math::Intervald::Closed(0., 0.), math::Intervald::Closed(0., 0.)); + EXPECT_NE(math::Intervald::Open(0., 1.), math::Intervald::Closed(0., 1.)); + EXPECT_NE( + math::Intervald::Open(0., 1.), math::Intervald::LeftClosed(0., 1.)); + EXPECT_NE( + math::Intervald::Open(0., 1.), math::Intervald::RightClosed(0., 1.)); + EXPECT_NE( + math::Intervald::Closed(0., 1.), math::Intervald::LeftClosed(0., 1.)); + EXPECT_NE( + math::Intervald::Closed(0., 1.), math::Intervald::RightClosed(0., 1.)); +} + +///////////////////////////////////////////////// +TEST(IntervalTest, IntervalIntersection) +{ + const math::Intervald openInterval = math::Intervald::Open(0., 1.); + EXPECT_FALSE(openInterval.Intersects(math::Intervald::Open(0.5, 0.5))); + EXPECT_TRUE(openInterval.Intersects(math::Intervald::Open(0.5, 1.5))); + EXPECT_TRUE(openInterval.Intersects(math::Intervald::Open(-0.5, 0.5))); + EXPECT_FALSE(openInterval.Intersects(math::Intervald::Closed(1., 1.))); + EXPECT_TRUE(openInterval.Intersects(math::Intervald::Closed(0.5, 0.5))); + EXPECT_FALSE(openInterval.Intersects(math::Intervald::Open(1., 2.))); + EXPECT_FALSE(openInterval.Intersects(math::Intervald::Open(-1., 0.))); + EXPECT_FALSE(openInterval.Intersects(math::Intervald::LeftClosed(1., 2.))); + EXPECT_FALSE(openInterval.Intersects(math::Intervald::RightClosed(-1., 0.))); + + const math::Intervald closedInterval = math::Intervald::Closed(0., 1.); + EXPECT_FALSE(closedInterval.Intersects(math::Intervald::Open(1., 1.))); + EXPECT_TRUE(closedInterval.Intersects(math::Intervald::Closed(0.5, 1.5))); + EXPECT_TRUE(closedInterval.Intersects(math::Intervald::Closed(-0.5, 0.5))); + EXPECT_FALSE(closedInterval.Intersects(math::Intervald::Closed(1.5, 2.5))); + EXPECT_FALSE(closedInterval.Intersects(math::Intervald::Closed(-1.5, -0.5))); + EXPECT_FALSE(closedInterval.Intersects(math::Intervald::Open(1., 2.))); + EXPECT_FALSE(closedInterval.Intersects(math::Intervald::Open(-1., 0.))); + EXPECT_TRUE(closedInterval.Intersects(math::Intervald::LeftClosed(1., 2.))); + EXPECT_TRUE(closedInterval.Intersects(math::Intervald::RightClosed(-1., 0.))); +} + +///////////////////////////////////////////////// +TEST(IntervalTest, IntervalStreaming) +{ + { + std::ostringstream os; + os << math::Intervald::Open(0., 1.); + EXPECT_EQ(os.str(), "(0, 1)"); + } + { + std::ostringstream os; + os << math::Intervald::LeftClosed(0., 1.); + EXPECT_EQ(os.str(), "[0, 1)"); + } + { + std::ostringstream os; + os << math::Intervald::RightClosed(0., 1.); + EXPECT_EQ(os.str(), "(0, 1]"); + } + { + std::ostringstream os; + os << math::Intervald::Closed(0., 1.); + EXPECT_EQ(os.str(), "[0, 1]"); + } +} From 1b84ea1cf9e57eeca4fdac6f355059f4bf4a44a9 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 21 Mar 2022 16:47:07 -0300 Subject: [PATCH 2/9] Add Region3 class (#390) Signed-off-by: Michel Hidalgo --- examples/CMakeLists.txt | 16 +-- examples/region3_example.cc | 61 +++++++++ include/ignition/math/Region3.hh | 208 +++++++++++++++++++++++++++++++ src/Region3_TEST.cc | 175 ++++++++++++++++++++++++++ 4 files changed, 446 insertions(+), 14 deletions(-) create mode 100644 examples/region3_example.cc create mode 100644 include/ignition/math/Region3.hh create mode 100644 src/Region3_TEST.cc diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index beb938796..1e1c19048 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -18,23 +18,11 @@ target_link_libraries(gauss_markov_process ignition-math${IGN_MATH_VER}::ignitio add_executable(vector2_example vector2_example.cc) target_link_libraries(vector2_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) -add_executable(graph_example graph_example.cc) -target_link_libraries(graph_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) - add_executable(interval_example interval_example.cc) target_link_libraries(interval_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) -add_executable(kmeans kmeans.cc) -target_link_libraries(kmeans ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) - -add_executable(quaternion_from_euler quaternion_from_euler.cc) -target_link_libraries(quaternion_from_euler ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) - -add_executable(quaternion_to_euler quaternion_to_euler.cc) -target_link_libraries(quaternion_to_euler ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) - -add_executable(rand_example rand_example.cc) -target_link_libraries(rand_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) +add_executable(region3_example region3_example.cc) +target_link_libraries(region3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) add_executable(triangle_example triangle_example.cc) target_link_libraries(triangle_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) diff --git a/examples/region3_example.cc b/examples/region3_example.cc new file mode 100644 index 000000000..1cb647c01 --- /dev/null +++ b/examples/region3_example.cc @@ -0,0 +1,61 @@ +/* + * 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. + * +*/ +//! [complete] +#include +#include +#include + +int main(int argc, char **argv) +{ + std::cout << std::boolalpha; + + const ignition::math::Region3d defaultRegion; + // A default constructed region should be empty. + std::cout << "The " << defaultRegion << " region is empty: " + << defaultRegion.Empty() << std::endl; + + const ignition::math::Region3d openRegion = + ignition::math::Region3d::Open(-1., -1., -1., 1., 1., 1.); + // An open region should exclude points on its boundary. + std::cout << "The " << openRegion << " region contains the " + << ignition::math::Vector3d::UnitX << " point: " + << openRegion.Contains(ignition::math::Vector3d::UnitX) + << std::endl; + + const ignition::math::Region3d closedRegion = + ignition::math::Region3d::Closed(0., 0., 0., 1., 1., 1.); + + // A closed region should include points on its boundary. + std::cout << "The " << closedRegion << " region contains the " + << ignition::math::Vector3d::UnitX << " point: " + << closedRegion.Contains(ignition::math::Vector3d::UnitX) + << std::endl; + + // Closed and open regions may intersect. + std::cout << "Regions " << closedRegion << " and " << openRegion + << " intersect: " << closedRegion.Intersects(openRegion) + << std::endl; + + // The unbounded region should include all non-empty regions. + std::cout << "The " << ignition::math::Region3d::Unbounded + << " region contains all previous non-empty intervals: " + << (ignition::math::Region3d::Unbounded.Contains(openRegion) || + ignition::math::Region3d::Unbounded.Contains(closedRegion)) + << std::endl; + +} +//! [complete] diff --git a/include/ignition/math/Region3.hh b/include/ignition/math/Region3.hh new file mode 100644 index 000000000..5d5bb4f8d --- /dev/null +++ b/include/ignition/math/Region3.hh @@ -0,0 +1,208 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_MATH_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: 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 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 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 + const 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/src/Region3_TEST.cc b/src/Region3_TEST.cc new file mode 100644 index 000000000..feb408919 --- /dev/null +++ b/src/Region3_TEST.cc @@ -0,0 +1,175 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include + +#include "ignition/math/Region3.hh" + +using namespace ignition; + +///////////////////////////////////////////////// +TEST(Region3Test, DefaultConstructor) +{ + const math::Region3d region; + EXPECT_TRUE(region.Ix().Empty()); + EXPECT_TRUE(region.Iy().Empty()); + EXPECT_TRUE(region.Iz().Empty()); +} + +///////////////////////////////////////////////// +TEST(Region3Test, Constructor) +{ + const math::Region3d region( + math::Intervald::Open(0., 1.), + math::Intervald::Closed(-1., 1.), + math::Intervald::Open(-1., 0.)); + EXPECT_EQ(region.Ix(), math::Intervald::Open(0., 1.)); + EXPECT_EQ(region.Iy(), math::Intervald::Closed(-1., 1.)); + EXPECT_EQ(region.Iz(), math::Intervald::Open(-1., 0.)); +} + +///////////////////////////////////////////////// +TEST(Region3Test, ConstructionHelpers) +{ + const math::Region3d openRegion = + math::Region3d::Open(0., 0., 0., 1., 1., 1.); + EXPECT_EQ(openRegion.Ix(), math::Intervald::Open(0., 1.)); + EXPECT_EQ(openRegion.Iy(), math::Intervald::Open(0., 1.)); + EXPECT_EQ(openRegion.Iz(), math::Intervald::Open(0., 1.)); + const math::Region3d closedRegion = + math::Region3d::Closed(0., 0., 0., 1., 1., 1.); + EXPECT_EQ(closedRegion.Ix(), math::Intervald::Closed(0., 1.)); + EXPECT_EQ(closedRegion.Iy(), math::Intervald::Closed(0., 1.)); + EXPECT_EQ(closedRegion.Iz(), math::Intervald::Closed(0., 1.)); +} + +///////////////////////////////////////////////// +TEST(Region3Test, UnboundedRegion) +{ + EXPECT_FALSE(math::Region3d::Unbounded.Empty()); + EXPECT_EQ(math::Region3d::Unbounded.Ix(), math::Intervald::Unbounded); + EXPECT_EQ(math::Region3d::Unbounded.Iy(), math::Intervald::Unbounded); + EXPECT_EQ(math::Region3d::Unbounded.Iz(), math::Intervald::Unbounded); +} + +///////////////////////////////////////////////// +TEST(Region3Test, EmptyRegion) +{ + EXPECT_FALSE(math::Region3d::Open(0., 0., 0., 1., 1., 1.).Empty()); + EXPECT_TRUE(math::Region3d::Open(0., 0., 0., 0., 0., 0.).Empty()); + EXPECT_TRUE(math::Region3d::Open(0., 0., 0., 0., 1., 1.).Empty()); + EXPECT_TRUE(math::Region3d::Open(0., 0., 0., 1., 0., 1.).Empty()); + EXPECT_TRUE(math::Region3d::Open(0., 0., 0., 1., 1., 0.).Empty()); + EXPECT_FALSE(math::Region3d::Closed(0., 0., 0., 0., 0., 0.).Empty()); + EXPECT_TRUE(math::Region3d::Closed(1., 1., 1., 0., 0., 0.).Empty()); +} + +///////////////////////////////////////////////// +TEST(Region3Test, RegionMembership) +{ + const math::Region3d openRegion = + math::Region3d::Open(0., 0., 0., 1., 1., 1.); + EXPECT_FALSE(openRegion.Contains(math::Vector3d(0., 0., 0.))); + EXPECT_TRUE(openRegion.Contains(math::Vector3d(0.5, 0.5, 0.5))); + EXPECT_FALSE(openRegion.Contains(math::Vector3d(1., 1., 1.))); + + const math::Region3d closedRegion = + math::Region3d::Closed(0., 0., 0., 1., 1., 1.); + EXPECT_TRUE(closedRegion.Contains(math::Vector3d(0., 0., 0.))); + EXPECT_TRUE(closedRegion.Contains(math::Vector3d(0.5, 0.5, 0.5))); + EXPECT_TRUE(closedRegion.Contains(math::Vector3d(1., 1., 1.))); +} + +///////////////////////////////////////////////// +TEST(Region3Test, RegionSubset) +{ + const math::Region3d openRegion = + math::Region3d::Open(0., 0., 0., 1., 1., 1.); + EXPECT_TRUE(openRegion.Contains( + math::Region3d::Open(0.25, 0.25, 0.25, + 0.75, 0.75, 0.75))); + EXPECT_FALSE(openRegion.Contains( + math::Region3d::Open(-1., 0.25, 0.25, + 0., 0.75, 0.75))); + EXPECT_FALSE(openRegion.Contains( + math::Region3d::Open(0.25, -1., 0.25, + 0.75, 0., 0.75))); + EXPECT_FALSE(openRegion.Contains( + math::Region3d::Open(0.25, 0.25, -1., + 0.75, 0.75, 0.))); + EXPECT_FALSE(openRegion.Contains( + math::Region3d::Closed(0., 0., 0., + 1., 1., 1.))); + + const math::Region3d closedRegion = + math::Region3d::Closed(0., 0., 0., 1., 1., 1.); + EXPECT_TRUE(closedRegion.Contains( + math::Region3d::Closed(0., 0., 0., 1., 1., 1.))); + EXPECT_TRUE(closedRegion.Contains( + math::Region3d::Closed(0., 0., 0., 0., 0., 0.))); +} + +///////////////////////////////////////////////// +TEST(Region3Test, RegionEquality) +{ + EXPECT_NE(math::Region3d::Open(0., 0., 0., 0., 0., 0.), + math::Region3d::Open(0., 0., 0., 0., 0., 0.)); + EXPECT_EQ(math::Region3d::Closed(0., 0., 0., 0., 0., 0.), + math::Region3d::Closed(0., 0., 0., 0., 0., 0.)); + EXPECT_NE(math::Region3d::Open(0., 0., 0., 1., 1., 1.), + math::Region3d::Closed(0., 0., 0., 1., 1., 1.)); +} + +///////////////////////////////////////////////// +TEST(Region3Test, RegionIntersection) +{ + const math::Region3d region = + math::Region3d::Open(0., 0., 0., 1., 1., 1.); + EXPECT_TRUE(region.Intersects( + math::Region3d::Open(0.5, 0.5, 0.5, 1.5, 1.5, 1.5))); + EXPECT_TRUE(region.Intersects( + math::Region3d::Open(-0.5, -0.5, -0.5, 0.5, 0.5, 0.5))); + EXPECT_FALSE(region.Intersects( + math::Region3d::Open(1., 1., 1., 2., 2., 2.))); + EXPECT_FALSE(region.Intersects( + math::Region3d::Open(-1., -1., -1., 0., 0., 0.))); +} + +///////////////////////////////////////////////// +TEST(IntervalTest, RegionStreaming) +{ + { + std::ostringstream os; + os << math::Region3d::Open(0., 1., 2., + 3., 4., 5.); + EXPECT_EQ(os.str(), "(0, 3) x (1, 4) x (2, 5)"); + } + { + std::ostringstream os; + os << math::Region3d::Closed(-1., 0., -1., + 0., 1., 0.); + EXPECT_EQ(os.str(), "[-1, 0] x [0, 1] x [-1, 0]"); + } + { + std::ostringstream os; + os << math::Region3d( + math::Intervald::LeftClosed(0., 100.), + math::Intervald::RightClosed(-100., 0.), + math::Intervald::Closed(0., 1.)); + EXPECT_EQ(os.str(), "[0, 100) x (-100, 0] x [0, 1]"); + } +} From 519e7945c3f6e97f7b2a132477f453ddd28b4845 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 23 Mar 2022 19:17:58 -0300 Subject: [PATCH 3/9] Add Polynomial3 class (#393) Signed-off-by: Michel Hidalgo Co-authored-by: Louise Poubel --- examples/CMakeLists.txt | 3 + examples/polynomial3_example.cc | 59 +++++ include/ignition/math/Polynomial3.hh | 295 +++++++++++++++++++++ src/Polynomial3_TEST.cc | 376 +++++++++++++++++++++++++++ 4 files changed, 733 insertions(+) create mode 100644 examples/polynomial3_example.cc create mode 100644 include/ignition/math/Polynomial3.hh create mode 100644 src/Polynomial3_TEST.cc diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 1e1c19048..bc440dbfa 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -21,6 +21,9 @@ target_link_libraries(vector2_example ignition-math${IGN_MATH_VER}::ignition-mat add_executable(interval_example interval_example.cc) target_link_libraries(interval_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) +add_executable(polynomial3_example polynomial3_example.cc) +target_link_libraries(polynomial3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + add_executable(region3_example region3_example.cc) target_link_libraries(region3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) diff --git a/examples/polynomial3_example.cc b/examples/polynomial3_example.cc new file mode 100644 index 000000000..f268dbdec --- /dev/null +++ b/examples/polynomial3_example.cc @@ -0,0 +1,59 @@ +/* + * 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. + * +*/ +//! [complete] +#include +#include +#include + +int main(int argc, char **argv) +{ + // A default constructed polynomial should have zero coefficients. + std::cout << "A default constructed polynomial is always: " + << ignition::math::Polynomial3d() << std::endl; + + // A constant polynomial only has an independent term. + std::cout << "A constant polynomial only has an independent term: " + << ignition::math::Polynomial3d::Constant(-1.) << std::endl; + + // A cubic polynomial may be incomplete. + const ignition::math::Polynomial3d p( + ignition::math::Vector4d(1., 0., -1., 2.)); + std::cout << "A cubic polynomial may be incomplete: " << p << std::endl; + + // A polynomial can be evaluated. + const double x = 0.5; + std::cout << "Evaluating " << p << " at " << x + << " yields " << p(x) << std::endl; + + // A polynomial can be queried for its minimum in a given interval, + // as well as for its global minimum (which may not always be finite). + const ignition::math::Intervald interval = + ignition::math::Intervald::Closed(-1, 1.); + std::cout << "The minimum of " << p << " in the " << interval + << " interval is " << p.Minimum(interval) << std::endl; + std::cout << "The global minimum of " << p + << " is " << p.Minimum() << std::endl; + + const ignition::math::Polynomial3d q( + ignition::math::Vector4d(0., 1., 2., 1.)); + std::cout << "The minimum of " << q << " in the " << interval + << " interval is " << q.Minimum(interval) << std::endl; + std::cout << "The global minimum of " << q + << " is " << q.Minimum() << std::endl; + +} +//! [complete] diff --git a/include/ignition/math/Polynomial3.hh b/include/ignition/math/Polynomial3.hh new file mode 100644 index 000000000..4112d8f80 --- /dev/null +++ b/include/ignition/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 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 diff --git a/src/Polynomial3_TEST.cc b/src/Polynomial3_TEST.cc new file mode 100644 index 000000000..dc5998cfa --- /dev/null +++ b/src/Polynomial3_TEST.cc @@ -0,0 +1,376 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include + +#include "ignition/math/Polynomial3.hh" + +using namespace ignition; + +///////////////////////////////////////////////// +TEST(Polynomial3Test, DefaultConstructor) +{ + const math::Polynomial3d poly; + EXPECT_EQ(poly.Coeffs(), math::Vector4d::Zero); +} + +///////////////////////////////////////////////// +TEST(Polynomial3Test, Constructor) +{ + const math::Polynomial3d poly(math::Vector4d::One); + EXPECT_EQ(poly.Coeffs(), math::Vector4d::One); +} + +///////////////////////////////////////////////// +TEST(Polynomial3Test, ConstructionHelpers) +{ + const math::Polynomial3d poly = math::Polynomial3d::Constant(1.); + EXPECT_EQ(poly.Coeffs(), math::Vector4d(0., 0., 0., 1.)); +} + +///////////////////////////////////////////////// +TEST(Polynomial3Test, Evaluate) +{ + { + const math::Polynomial3d p = + math::Polynomial3d::Constant(1.); + EXPECT_DOUBLE_EQ(p(-1.), 1.); + EXPECT_DOUBLE_EQ(p(0.), 1.); + EXPECT_DOUBLE_EQ(p(1.), 1.); + EXPECT_DOUBLE_EQ(p(math::INF_D), 1.); + EXPECT_TRUE(std::isnan(p(math::NAN_D))); + } + { + const math::Polynomial3d p(math::Vector4d::One); + EXPECT_DOUBLE_EQ(p(-1.), 0.); + EXPECT_DOUBLE_EQ(p(0.), 1.); + EXPECT_DOUBLE_EQ(p(1.), 4.); + EXPECT_DOUBLE_EQ(p(-math::INF_D), -math::INF_D); + EXPECT_TRUE(std::isnan(p(math::NAN_D))); + } +} + +///////////////////////////////////////////////// +TEST(Polynomial3Test, Minimum) +{ + { + const math::Polynomial3d p = + math::Polynomial3d::Constant(1.); + const math::Intervald xInterval = + math::Intervald::Open(0., 0.); + double xMin = 0.; + EXPECT_TRUE(std::isnan(p.Minimum(xInterval))); + EXPECT_TRUE(std::isnan(p.Minimum(xInterval, xMin))); + EXPECT_TRUE(std::isnan(xMin)); + } + { + const math::Polynomial3d p = + math::Polynomial3d::Constant(1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), 1.); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), 1.); + EXPECT_FALSE(std::isnan(xMin)); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 0., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(0., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 1.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 1.); + EXPECT_DOUBLE_EQ(xMin, 0.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 0., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, -math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 0., -1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(0., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 0.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 0.); + EXPECT_DOUBLE_EQ(xMin, 1.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 0., -1., 1.)); + double xMin = 0.; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(1., 2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 3.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 3.); + EXPECT_DOUBLE_EQ(xMin, 1.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-1., 0.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 0.75); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 0.75); + EXPECT_DOUBLE_EQ(xMin, -0.5); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-3., -2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 3.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 3.); + EXPECT_DOUBLE_EQ(xMin, -2.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., 1., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), 0.75); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), 0.75); + EXPECT_DOUBLE_EQ(xMin, -0.5); + } + { + const math::Polynomial3d p( + math::Vector4d(0., -1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(1., 2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -1.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -1.); + EXPECT_DOUBLE_EQ(xMin, 2.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., -1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-2., -1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -5.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -5.); + EXPECT_DOUBLE_EQ(xMin, -2.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., -1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(0., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 1.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 1.); + EXPECT_DOUBLE_EQ(xMin, 0.); + } + { + const math::Polynomial3d p( + math::Vector4d(0., -1., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, -math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(1., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-1., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 0.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 0.); + EXPECT_DOUBLE_EQ(xMin, -1.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-2., -1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -5.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -5.); + EXPECT_DOUBLE_EQ(xMin, -2.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., 1., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(2., 3.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 15.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 15.); + EXPECT_DOUBLE_EQ(xMin, 2.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., 1., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, -math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(-1., 2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-1., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 0.8873882090776197); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 0.8873882090776197); + EXPECT_DOUBLE_EQ(xMin, -0.2152504370215302); + } + { + const math::Polynomial3d p( + math::Vector4d(-1., 2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-3., -2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 15.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 15.); + EXPECT_DOUBLE_EQ(xMin, -2.); + } + { + const math::Polynomial3d p( + math::Vector4d(-1., 2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(1., 2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 3.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 3.); + EXPECT_DOUBLE_EQ(xMin, 1.); + } + { + const math::Polynomial3d p( + math::Vector4d(-1., 2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(2., 3.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -5.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -5.); + EXPECT_DOUBLE_EQ(xMin, 3.); + } + { + const math::Polynomial3d p( + math::Vector4d(-1., 2., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(-1., 1.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -3.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -3.); + EXPECT_DOUBLE_EQ(xMin, -1.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(0., 2.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 1.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 1.); + EXPECT_DOUBLE_EQ(xMin, 0.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -2., 1., 1.)); + const math::Intervald xInterval = + math::Intervald::Open(2., 3.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), 3.); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), 3.); + EXPECT_DOUBLE_EQ(xMin, 2.); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -2., 1., 1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, -math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -4., -2., -1.)); + const math::Intervald xInterval = + math::Intervald::Open(-1., 6.); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(xInterval), -16.051047904897441); + EXPECT_DOUBLE_EQ(p.Minimum(xInterval, xMin), -16.051047904897441); + EXPECT_DOUBLE_EQ(xMin, 2.8968052532744766); + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + } + { + const math::Polynomial3d p( + math::Vector4d(1., -4., -2., -1.)); + double xMin = math::NAN_D; + EXPECT_DOUBLE_EQ(p.Minimum(), -math::INF_D); + EXPECT_DOUBLE_EQ(p.Minimum(xMin), -math::INF_D); + EXPECT_DOUBLE_EQ(xMin, -math::INF_D); + } +} + +///////////////////////////////////////////////// +TEST(Polynomial3Test, PolynomialStreaming) +{ + { + std::ostringstream os; + os << math::Polynomial3d(math::Vector4d::Zero); + EXPECT_EQ(os.str(), "0"); + } + { + std::ostringstream os; + os << math::Polynomial3d(math::Vector4d::One); + EXPECT_EQ(os.str(), "x^3 + x^2 + x + 1"); + } + { + std::ostringstream os; + os << math::Polynomial3d( + math::Vector4d(1., 0., 1., 0.)); + EXPECT_EQ(os.str(), "x^3 + x"); + } + { + std::ostringstream os; + os << math::Polynomial3d( + math::Vector4d(0., 1., 0., -1.)); + EXPECT_EQ(os.str(), "x^2 - 1"); + } + { + std::ostringstream os; + os << math::Polynomial3d( + math::Vector4d(-1., 2., -2., 0.)); + EXPECT_EQ(os.str(), "-x^3 + 2 x^2 - 2 x"); + } +} From 6a6cbd4d8912c35c6c017f78d5bcad58df5ce10e Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 29 Mar 2022 20:21:30 -0300 Subject: [PATCH 4/9] Add AdditivelySeparableScalarField3 class (#397) Signed-off-by: Michel Hidalgo Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel --- examples/CMakeLists.txt | 3 + ...itively_separable_scalar_field3_example.cc | 52 +++++ .../math/AdditivelySeparableScalarField3.hh | 197 ++++++++++++++++++ src/AdditivelySeparableScalarField3_TEST.cc | 112 ++++++++++ 4 files changed, 364 insertions(+) create mode 100644 examples/additively_separable_scalar_field3_example.cc create mode 100644 include/ignition/math/AdditivelySeparableScalarField3.hh create mode 100644 src/AdditivelySeparableScalarField3_TEST.cc diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index bc440dbfa..edbb56e8b 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -6,6 +6,9 @@ project(ignition-math-examples) set(IGN_MATH_VER 6) find_package(ignition-math${IGN_MATH_VER} REQUIRED) +add_executable(additively_separable_scalar_field3_example additively_separable_scalar_field3_example.cc) +target_link_libraries(additively_separable_scalar_field3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + add_executable(angle_example angle_example.cc) target_link_libraries(angle_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) diff --git a/examples/additively_separable_scalar_field3_example.cc b/examples/additively_separable_scalar_field3_example.cc new file mode 100644 index 000000000..c46ffd5ee --- /dev/null +++ b/examples/additively_separable_scalar_field3_example.cc @@ -0,0 +1,52 @@ +/* + * 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. + * +*/ +//! [complete] +#include +#include +#include + +int main(int argc, char **argv) +{ + const double kConstant = 1.; + const ignition::math::Polynomial3d xPoly( + ignition::math::Vector4d(0., 1., 0., 1.)); + const ignition::math::Polynomial3d yPoly( + ignition::math::Vector4d(1., 0., 1., 0.)); + const ignition::math::Polynomial3d zPoly( + ignition::math::Vector4d(1., 0., 0., -1.)); + using AdditivelySeparableScalarField3dT = + ignition::math::AdditivelySeparableScalarField3d< + ignition::math::Polynomial3d>; + const AdditivelySeparableScalarField3dT scalarField( + kConstant, xPoly, yPoly, zPoly); + + // A printable, additively separable scalar field. + std::cout << "An additively separable scalar field in R^3 " + << "can be expressed as a sum of scalar functions " + << "e.g. F(x, y, z) = " << scalarField << std::endl; + + // An additively separable scalar field can be evaluated. + std::cout << "Evaluating F(x, y, z) at (0, 1, 0) yields " + << scalarField(ignition::math::Vector3d::UnitY) + << std::endl; + + // An additively separable scalar field can be queried for its + // minimum (provided the underlying scalar function allows it). + std::cout << "The global minimum of F(x, y, z) is " + << scalarField.Minimum() << std::endl; +} +//! [complete] diff --git a/include/ignition/math/AdditivelySeparableScalarField3.hh b/include/ignition/math/AdditivelySeparableScalarField3.hh new file mode 100644 index 000000000..e05128a35 --- /dev/null +++ b/include/ignition/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 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 diff --git a/src/AdditivelySeparableScalarField3_TEST.cc b/src/AdditivelySeparableScalarField3_TEST.cc new file mode 100644 index 000000000..bbd52d323 --- /dev/null +++ b/src/AdditivelySeparableScalarField3_TEST.cc @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include +#include + +#include "ignition/math/AdditivelySeparableScalarField3.hh" +#include "ignition/math/Polynomial3.hh" + +using namespace ignition; + +///////////////////////////////////////////////// +TEST(AdditivelySeparableScalarField3Test, Evaluate) +{ + using ScalarFunctionT = std::function; + using AdditivelySeparableScalarField3dT = + math::AdditivelySeparableScalarField3d; + const double kConstant = 1.; + auto xyzFunc = [](double x) { return x; }; + const AdditivelySeparableScalarField3dT scalarField( + kConstant, xyzFunc, xyzFunc, xyzFunc); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::Zero), 0.); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::One), 3.); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::UnitX), 1.); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::UnitY), 1.); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::UnitZ), 1.); + const math::Vector3d INF_V( + math::INF_D, math::INF_D, math::INF_D); + EXPECT_DOUBLE_EQ(scalarField(INF_V), math::INF_D); +} + +///////////////////////////////////////////////// +TEST(AdditivelySeparableScalarField3Test, Minimum) +{ + using AdditivelySeparableScalarField3dT = + math::AdditivelySeparableScalarField3d; + constexpr double kConstant = 1. / 3.; + const math::Polynomial3d xyzPoly(math::Vector4d(0., 1., 1., 1)); + const AdditivelySeparableScalarField3dT scalarField( + kConstant, xyzPoly, xyzPoly, xyzPoly); + { + const math::Region3d region = + math::Region3d::Open(0., 0., 0., 0., 0., 0.); + math::Vector3d pMin = math::Vector3d::Zero; + EXPECT_TRUE(std::isnan(scalarField.Minimum(region))); + EXPECT_TRUE(std::isnan(scalarField.Minimum(region, pMin))); + EXPECT_TRUE(std::isnan(pMin.X())); + EXPECT_TRUE(std::isnan(pMin.Y())); + EXPECT_TRUE(std::isnan(pMin.Z())); + } + { + math::Vector3d pMin = math::Vector3d::NaN; + EXPECT_DOUBLE_EQ(scalarField.Minimum(), 0.75); + EXPECT_DOUBLE_EQ(scalarField.Minimum(pMin), 0.75); + EXPECT_EQ(pMin, -0.5 * math::Vector3d::One); + } + { + const math::Region3d region = + math::Region3d::Open(1., 1., 1., 2., 2., 2.); + math::Vector3d pMin = math::Vector3d::NaN; + EXPECT_DOUBLE_EQ(scalarField.Minimum(region), 3.); + EXPECT_DOUBLE_EQ(scalarField.Minimum(region, pMin), 3.); + EXPECT_EQ(pMin, math::Vector3d::One); + } +} + +///////////////////////////////////////////////// +TEST(AdditivelySeparableScalarField3Test, Stream) +{ + using AdditivelySeparableScalarField3dT = + math::AdditivelySeparableScalarField3d; + { + constexpr double kConstant = 1.; + const math::Polynomial3d xyzPoly(math::Vector4d(0., 1., 0., 1)); + std::ostringstream os; + os << AdditivelySeparableScalarField3dT( + kConstant, xyzPoly, xyzPoly, xyzPoly); + EXPECT_EQ(os.str(), "[(x^2 + 1) + (y^2 + 1) + (z^2 + 1)]"); + } + { + constexpr double kConstant = -1.; + const math::Polynomial3d xyzPoly(math::Vector4d(1., 0., 1., 0)); + std::ostringstream os; + os << AdditivelySeparableScalarField3dT( + kConstant, xyzPoly, xyzPoly, xyzPoly); + EXPECT_EQ(os.str(), "-[(x^3 + x) + (y^3 + y) + (z^3 + z)]"); + } + { + constexpr double kConstant = 3.; + const math::Polynomial3d xPoly(math::Vector4d(0., 1., 0., 0)); + const math::Polynomial3d yPoly(math::Vector4d(-1., 0., 0., 0)); + const math::Polynomial3d zPoly(math::Vector4d(0., 0., 0., 1)); + std::ostringstream os; + os << AdditivelySeparableScalarField3dT( + kConstant, xPoly, yPoly, zPoly); + EXPECT_EQ(os.str(), "3 [(x^2) + (-y^3) + (1)]"); + } +} From 17e8eff34a425ca6f3b286d1b155b3db22e9438f Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 30 Mar 2022 16:32:43 -0300 Subject: [PATCH 5/9] Fix unsafe globals in Interval and Region3 (#396) Signed-off-by: Michel Hidalgo --- include/ignition/math/Interval.hh | 25 ++++++++++++++++--------- include/ignition/math/Region3.hh | 9 +++++---- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/include/ignition/math/Interval.hh b/include/ignition/math/Interval.hh index fb0229b10..115db95dc 100644 --- a/include/ignition/math/Interval.hh +++ b/include/ignition/math/Interval.hh @@ -51,11 +51,14 @@ namespace ignition /// \brief Constructor /// \param[in] _leftValue leftmost interval value - /// \param[in] _leftClosed whether the interval is left-closed or not + /// \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: Interval(T _leftValue, bool _leftClosed, - T _rightValue, bool _rightClosed) + /// \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), @@ -67,7 +70,8 @@ namespace ignition /// \param[in] _leftValue leftmost interval value /// \param[in] _rightValue rightmost interval value /// \return the open interval - public: static Interval Open(T _leftValue, T _rightValue) + public: static constexpr Interval + Open(T _leftValue, T _rightValue) { return Interval( std::move(_leftValue), false, @@ -78,7 +82,8 @@ namespace ignition /// \param[in] _leftValue leftmost interval value /// \param[in] _rightValue rightmost interval value /// \return the left-closed interval - public: static Interval LeftClosed(T _leftValue, T _rightValue) + public: static constexpr Interval + LeftClosed(T _leftValue, T _rightValue) { return Interval( std::move(_leftValue), true, @@ -89,7 +94,8 @@ namespace ignition /// \param[in] _leftValue leftmost interval value /// \param[in] _rightValue rightmost interval value /// \return the left-closed interval - public: static Interval RightClosed(T _leftValue, T _rightValue) + public: static constexpr Interval + RightClosed(T _leftValue, T _rightValue) { return Interval( std::move(_leftValue), false, @@ -100,7 +106,8 @@ namespace ignition /// \param[in] _leftValue leftmost interval value /// \param[in] _rightValue rightmost interval value /// \return the closed interval - public: static Interval Closed(T _leftValue, T _rightValue) + public: static constexpr Interval + Closed(T _leftValue, T _rightValue) { return Interval{ std::move(_leftValue), true, @@ -276,7 +283,7 @@ namespace ignition namespace detail { template - const Interval gUnboundedInterval = + constexpr Interval gUnboundedInterval = Interval::Open(-std::numeric_limits::infinity(), std::numeric_limits::infinity()); } // namespace detail diff --git a/include/ignition/math/Region3.hh b/include/ignition/math/Region3.hh index 5d5bb4f8d..339a9d91b 100644 --- a/include/ignition/math/Region3.hh +++ b/include/ignition/math/Region3.hh @@ -60,7 +60,8 @@ namespace ignition /// \param[in] _ix x-axis interval /// \param[in] _iy y-axis interval /// \param[in] _iz z-axis interval - public: Region3(Interval _ix, Interval _iy, Interval _iz) + public: constexpr Region3( + Interval _ix, Interval _iy, Interval _iz) : ix(std::move(_ix)), iy(std::move(_iy)), iz(std::move(_iz)) { } @@ -74,7 +75,7 @@ namespace ignition /// \param[in] _zRight righmost z-axis interval value /// \return the (`_xLeft`, `_xRight`) ✕ (`_yLeft`, `_yRight`) /// ✕ (`_zLeft`, `_zRight`) open region - public: static Region3 Open( + public: static constexpr Region3 Open( T _xLeft, T _yLeft, T _zLeft, T _xRight, T _yRight, T _zRight) { @@ -92,7 +93,7 @@ namespace ignition /// \param[in] _zRight righmost z-axis interval value /// \return the [`_xLeft`, `_xRight`] ✕ [`_yLeft`, `_yRight`] /// ✕ [`_zLeft`, `_zRight`] closed region - public: static Region3 Closed( + public: static constexpr Region3 Closed( T _xLeft, T _yLeft, T _zLeft, T _xRight, T _yRight, T _zRight) { @@ -188,7 +189,7 @@ namespace ignition namespace detail { template - const Region3 gUnboundedRegion3( + constexpr Region3 gUnboundedRegion3( Interval::Open(-std::numeric_limits::infinity(), std::numeric_limits::infinity()), Interval::Open(-std::numeric_limits::infinity(), From 5bd8576048e22a8166df15daacd7399493a54ad7 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Fri, 1 Apr 2022 13:25:45 -0300 Subject: [PATCH 6/9] Add PiecewiseScalarField3 class (#398) Signed-off-by: Michel Hidalgo --- examples/CMakeLists.txt | 4 + examples/piecewise_scalar_field3_example.cc | 73 ++++++ .../ignition/math/PiecewiseScalarField3.hh | 219 ++++++++++++++++++ src/PiecewiseScalarField3_TEST.cc | 185 +++++++++++++++ 4 files changed, 481 insertions(+) create mode 100644 examples/piecewise_scalar_field3_example.cc create mode 100644 include/ignition/math/PiecewiseScalarField3.hh create mode 100644 src/PiecewiseScalarField3_TEST.cc diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index edbb56e8b..91c23d8f6 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -24,6 +24,10 @@ target_link_libraries(vector2_example ignition-math${IGN_MATH_VER}::ignition-mat add_executable(interval_example interval_example.cc) target_link_libraries(interval_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + +add_executable(piecewise_scalar_field3_example piecewise_scalar_field3_example.cc) +target_link_libraries(piecewise_scalar_field3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + add_executable(polynomial3_example polynomial3_example.cc) target_link_libraries(polynomial3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) diff --git a/examples/piecewise_scalar_field3_example.cc b/examples/piecewise_scalar_field3_example.cc new file mode 100644 index 000000000..d584ff09c --- /dev/null +++ b/examples/piecewise_scalar_field3_example.cc @@ -0,0 +1,73 @@ +/* + * 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. + * +*/ +//! [complete] +#include + +#include +#include +#include + +int main(int argc, char **argv) +{ + const double kConstant = 1.; + const ignition::math::Polynomial3d xPoly( + ignition::math::Vector4d(0., 1., 0., 1.)); + const ignition::math::Polynomial3d yPoly( + ignition::math::Vector4d(1., 0., 1., 0.)); + const ignition::math::Polynomial3d zPoly( + ignition::math::Vector4d(1., 0., 0., -1.)); + using AdditivelySeparableScalarField3dT = + ignition::math::AdditivelySeparableScalarField3d< + ignition::math::Polynomial3d>; + using PiecewiseScalarField3dT = + ignition::math::PiecewiseScalarField3d< + AdditivelySeparableScalarField3dT>; + const PiecewiseScalarField3dT scalarField({ + {ignition::math::Region3d( // x < 0 halfspace + ignition::math::Intervald::Open( + -ignition::math::INF_D, 0.), + ignition::math::Intervald::Unbounded, + ignition::math::Intervald::Unbounded), + AdditivelySeparableScalarField3dT( + kConstant, xPoly, yPoly, zPoly)}, + {ignition::math::Region3d( // x >= 0 halfspace + ignition::math::Intervald::LeftClosed( + 0., ignition::math::INF_D), + ignition::math::Intervald::Unbounded, + ignition::math::Intervald::Unbounded), + AdditivelySeparableScalarField3dT( + -kConstant, xPoly, yPoly, zPoly)}}); + + // A printable piecewise scalar field. + std::cout << "A piecewise scalar field in R^3 is made up of " + << "several pieces e.g. P(x, y, z) = " + << scalarField << std::endl; + + // A piecewise scalar field can be evaluated. + std::cout << "Evaluating P(x, y, z) at (1, 0, 0) yields " + << scalarField(ignition::math::Vector3d::UnitX) + << std::endl; + std::cout << "Evaluating P(x, y, z) at (-1, 0, 0) yields " + << scalarField(-ignition::math::Vector3d::UnitX) + << std::endl; + + // A piecewise scalar field can be queried for its minimum + // (provided the underlying scalar function allows it). + std::cout << "The global minimum of P(x, y, z) is " + << scalarField.Minimum() << std::endl; +} +//! [complete] diff --git a/include/ignition/math/PiecewiseScalarField3.hh b/include/ignition/math/PiecewiseScalarField3.hh new file mode 100644 index 000000000..ce938d1bf --- /dev/null +++ b/include/ignition/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 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 diff --git a/src/PiecewiseScalarField3_TEST.cc b/src/PiecewiseScalarField3_TEST.cc new file mode 100644 index 000000000..ac7c10c1f --- /dev/null +++ b/src/PiecewiseScalarField3_TEST.cc @@ -0,0 +1,185 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include + +#include +#include + +#include "ignition/math/AdditivelySeparableScalarField3.hh" +#include "ignition/math/PiecewiseScalarField3.hh" +#include "ignition/math/Polynomial3.hh" +#include "ignition/math/Vector3.hh" + +using namespace ignition; + +///////////////////////////////////////////////// +TEST(PiecewiseScalarField3Test, Evaluate) +{ + using ScalarField3dT = std::function; + using PiecewiseScalarField3dT = math::PiecewiseScalarField3d; + { + const PiecewiseScalarField3dT scalarField; + EXPECT_TRUE(std::isnan(scalarField(math::Vector3d::Zero))); + } + { + const PiecewiseScalarField3dT scalarField = + PiecewiseScalarField3dT::Throughout( + [](const math::Vector3d& v) { return v.X(); }); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::Zero), 0.); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d(0.5, 0.5, 0.5)), 0.5); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::One), 1.); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::UnitX), 1.); + } + { + const math::Region3d region0 = + math::Region3d::Open(0., 0., 0., 0., 0., 0.); + auto field0 = [](const math::Vector3d&) { return 1.; }; + const math::Region3d region1 = + math::Region3d::Closed(0., 0., 0., 1., 1., 1.); + auto field1 = [](const math::Vector3d& v) { return v.X(); }; + const PiecewiseScalarField3dT scalarField({ + {region0, field0}, {region1, field1}}); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::Zero), 0.); + } + { + const math::Region3d region0 = + math::Region3d::Closed(0., 0., 0., 0., 0., 0.); + auto field0 = [](const math::Vector3d&) { return 1.; }; + const math::Region3d region1 = + math::Region3d::Closed(0., 0., 0., 1., 1., 1.); + auto field1 = [](const math::Vector3d& v) { return v.X(); }; + const PiecewiseScalarField3dT scalarField({ + {region0, field0}, {region1, field1}}); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::Zero), 1.); + } + { + const math::Region3d region0 = + math::Region3d::Open(0., 0., 0., 1., 1., 1.); + auto field0 = [](const math::Vector3d& v) { return v.X(); }; + const math::Region3d region1 = + math::Region3d::Open(-1., -1., -1., 0., 0., 0.); + auto field1 = [](const math::Vector3d& v) { return v.Y(); }; + const PiecewiseScalarField3dT scalarField({ + {region0, field0}, {region1, field1}}); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d(0.5, 0.25, 0.5)), 0.5); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d(-0.5, -0.25, -0.5)), -0.25); + EXPECT_TRUE(std::isnan(scalarField(math::Vector3d(0.5, -0.25, 0.5)))); + EXPECT_TRUE(std::isnan(scalarField(math::Vector3d(-0.5, 0.25, -0.5)))); + } +} + +///////////////////////////////////////////////// +TEST(PiecewiseScalarField3Test, Minimum) +{ + using AdditivelySeparableScalarField3dT = + math::AdditivelySeparableScalarField3d; + using PiecewiseScalarField3dT = + math::PiecewiseScalarField3d; + { + const PiecewiseScalarField3dT scalarField; + math::Vector3d pMin = math::Vector3d::Zero; + EXPECT_TRUE(std::isnan(scalarField.Minimum())); + EXPECT_TRUE(std::isnan(scalarField.Minimum(pMin))); + EXPECT_TRUE(std::isnan(pMin.X())); + EXPECT_TRUE(std::isnan(pMin.Y())); + EXPECT_TRUE(std::isnan(pMin.Z())); + } + { + const PiecewiseScalarField3dT scalarField = + PiecewiseScalarField3dT::Throughout( + AdditivelySeparableScalarField3dT( + 1., + math::Polynomial3d::Constant(0.), + math::Polynomial3d::Constant(1.), + math::Polynomial3d::Constant(0.))); + math::Vector3d pMin = math::Vector3d::NaN; + EXPECT_DOUBLE_EQ(scalarField.Minimum(), 1.); + EXPECT_DOUBLE_EQ(scalarField.Minimum(pMin), 1.); + EXPECT_FALSE(std::isnan(pMin.X())); + EXPECT_FALSE(std::isnan(pMin.Y())); + EXPECT_FALSE(std::isnan(pMin.Z())); + } + { + const math::Region3d region0 = + math::Region3d::Open(0., 0., 0., 1., 1., 1.); + const AdditivelySeparableScalarField3dT field0( + 1., + math::Polynomial3d( + math::Vector4d(0., 1., 0., 0.)), + math::Polynomial3d::Constant(0.), + math::Polynomial3d::Constant(0.)); + const math::Region3d region1 = + math::Region3d::Open(-1., -1., -1., 0., 0., 0.); + const AdditivelySeparableScalarField3dT field1( + 1., + math::Polynomial3d::Constant(0.), + math::Polynomial3d( + math::Vector4d(0., 1., 0., 1.)), + math::Polynomial3d::Constant(0.)); + const PiecewiseScalarField3dT scalarField({ + {region0, field0}, {region1, field1}}); + math::Vector3d pMin = math::Vector3d::NaN; + EXPECT_DOUBLE_EQ(scalarField.Minimum(), 0.); + EXPECT_DOUBLE_EQ(scalarField.Minimum(pMin), 0.); + EXPECT_EQ(pMin, math::Vector3d::Zero); + } +} + + +///////////////////////////////////////////////// +TEST(PiecewiseScalarField3Test, Stream) +{ + using AdditivelySeparableScalarField3dT = + math::AdditivelySeparableScalarField3d; + using PiecewiseScalarField3dT = + math::PiecewiseScalarField3d; + { + std::ostringstream output; + output << PiecewiseScalarField3dT(); + EXPECT_EQ(output.str(), "undefined"); + } + { + std::ostringstream output; + std::ostringstream expected; + const math::Region3d region0( + math::Intervald::Unbounded, + math::Intervald::Unbounded, + math::Intervald::Open(-math::INF_D, 0.)); + const AdditivelySeparableScalarField3dT field0( + 1., + math::Polynomial3d(math::Vector4d(0., 1., 0., 0.)), + math::Polynomial3d(math::Vector4d(0., 1., 0., 0.)), + math::Polynomial3d(math::Vector4d(0., 1., 0., 0.))); + expected << "[(x^2) + (y^2) + (z^2)] if (x, y, z) " + << "in (-inf, inf) x (-inf, inf) x (-inf, 0); "; + const math::Region3d region1( + math::Intervald::Unbounded, + math::Intervald::Unbounded, + math::Intervald::LeftClosed(0., math::INF_D)); + const AdditivelySeparableScalarField3dT field1( + -1., + math::Polynomial3d(math::Vector4d(0., 0., 1., 0.)), + math::Polynomial3d(math::Vector4d(0., 0., 1., 0.)), + math::Polynomial3d(math::Vector4d(0., 0., 1., 0.))); + expected << "-[(x) + (y) + (z)] if (x, y, z) " + << "in (-inf, inf) x (-inf, inf) x [0, inf)"; + const PiecewiseScalarField3dT scalarField({ + {region0, field0}, {region1, field1}}); + output << scalarField; + EXPECT_EQ(output.str(), expected.str()); + } +} From 43188b6931024636d3b43e48f2fc660064adcade Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Mon, 4 Jul 2022 11:05:16 +0100 Subject: [PATCH 7/9] Added IntervalPython interface MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- src/python_pybind11/CMakeLists.txt | 2 + src/python_pybind11/src/Interval.cc | 34 ++++ src/python_pybind11/src/Interval.hh | 118 ++++++++++++++ .../src/_ignition_math_pybind11.cc | 3 + src/python_pybind11/test/Interval_TEST.py | 151 ++++++++++++++++++ 5 files changed, 308 insertions(+) create mode 100644 src/python_pybind11/src/Interval.cc create mode 100644 src/python_pybind11/src/Interval.hh create mode 100644 src/python_pybind11/test/Interval_TEST.py diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 5967f17dc..9729827ac 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -19,6 +19,7 @@ pybind11_add_module(math SHARED src/Frustum.cc src/GaussMarkovProcess.cc src/Helpers.cc + src/Interval.cc src/Kmeans.cc src/Line2.cc src/Line3.cc @@ -110,6 +111,7 @@ if (BUILD_TESTING) GaussMarkovProcess_TEST Helpers_TEST Inertial_TEST + Interval_TEST Kmeans_TEST Line2_TEST Line3_TEST diff --git a/src/python_pybind11/src/Interval.cc b/src/python_pybind11/src/Interval.cc new file mode 100644 index 000000000..92165d73f --- /dev/null +++ b/src/python_pybind11/src/Interval.cc @@ -0,0 +1,34 @@ +/* + * 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 "Interval.hh" + +namespace ignition +{ +namespace math +{ +namespace python +{ +void defineMathInterval(py::module &m, const std::string &typestr) +{ + helpDefineMathInterval(m, typestr + "f"); + helpDefineMathInterval(m, typestr + "d"); +} + +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/Interval.hh b/src/python_pybind11/src/Interval.hh new file mode 100644 index 000000000..7539bff31 --- /dev/null +++ b/src/python_pybind11/src/Interval.hh @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_MATH_PYTHON__INTERVAL_HH_ +#define IGNITION_MATH_PYTHON__INTERVAL_HH_ + +#include +#include + +#include +#include + +#include + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Help define a pybind11 wrapper for an ignition::math::Interval +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void helpDefineMathInterval(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::Interval; + auto toString = [](const Class &si) { + std::stringstream stream; + stream << si; + return stream.str(); + }; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init()) + .def(py::self != py::self) + .def(py::self == py::self) + .def("open", + &Class::Open, + "Make an open interval (`_leftValue`, `_rightValue`)") + .def("left_closed", + &Class::LeftClosed, + "Make a left-closed interval [`_leftValue`, `_rightValue`)") + .def("right_closed", + &Class::RightClosed, + "Make a right-closed interval (`_leftValue`, `_rightValue`]") + .def("closed", + &Class::Closed, + "Make a closed interval [`_leftValue`, `_rightValue`]") + .def("left_value", + &Class::LeftValue, + "Get the leftmost interval value") + .def("is_left_closed", + &Class::IsLeftClosed, + "Check if the interval is left-closed") + .def("right_value", + &Class::RightValue, + "Get the rightmost interval value") + .def("is_right_closed", + &Class::IsRightClosed, + "Check if the interval is right-closed") + .def("empty", + &Class::Empty, + "Check if the interval is empty") + .def("contains", + py::overload_cast(&Class::Contains, py::const_), + "Check if the interval contains `_value`") + .def("contains", + py::overload_cast(&Class::Contains, py::const_), + "Check if the interval contains `_other` interval") + .def("intersects", + &Class::Intersects, + "Check if the interval intersects `_other` interval") + .def("__copy__", [](const Class &self) { + return Class(self); + }) + .def("__deepcopy__", [](const Class &self, py::dict) { + return Class(self); + }, "memo"_a) + .def("__str__", toString) + .def("__repr__", toString); +} + +/// Define a pybind11 wrapper for an ignition::math::Interval +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathInterval(py::module &m, const std::string &typestr); + +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__INTERVAL_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index 00497c61c..1c9bf494c 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -27,6 +27,7 @@ #include "GaussMarkovProcess.hh" #include "Helpers.hh" #include "Inertial.hh" +#include "Interval.hh" #include "Kmeans.hh" #include "Line2.hh" #include "Line3.hh" @@ -131,6 +132,8 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathVector4(m, "Vector4"); + ignition::math::python::defineMathInterval(m, "Interval"); + ignition::math::python::defineMathLine2(m, "Line2"); ignition::math::python::defineMathLine3(m, "Line3"); diff --git a/src/python_pybind11/test/Interval_TEST.py b/src/python_pybind11/test/Interval_TEST.py new file mode 100644 index 000000000..cef97c4fd --- /dev/null +++ b/src/python_pybind11/test/Interval_TEST.py @@ -0,0 +1,151 @@ +# Copyright (C) 2022 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License") +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +import unittest +from ignition.math import Intervald + + +class TestInterval(unittest.TestCase): + + def test_default_constructor(self): + interval = Intervald() + self.assertAlmostEqual(interval.left_value(), interval.right_value()) + + def test_construct(self): + kClosed = True + + interval = Intervald(0., kClosed, 1., not kClosed) + self.assertAlmostEqual(interval.left_value(), 0.) + self.assertTrue(interval.is_left_closed()) + self.assertAlmostEqual(interval.right_value(), 1.) + self.assertFalse(interval.is_right_closed()) + + def test_construction_helpers(self): + openInterval = Intervald.open(0., 1.) + self.assertAlmostEqual(openInterval.left_value(), 0.) + self.assertFalse(openInterval.is_left_closed()) + self.assertAlmostEqual(openInterval.right_value(), 1.) + self.assertFalse(openInterval.is_right_closed()) + + leftClosedInterval = Intervald.left_closed(0., 1.) + self.assertAlmostEqual(leftClosedInterval.left_value(), 0.) + self.assertTrue(leftClosedInterval.is_left_closed()) + self.assertAlmostEqual(leftClosedInterval.right_value(), 1.) + self.assertFalse(leftClosedInterval.is_right_closed()) + + rightClosedInterval = Intervald.right_closed(0., 1.) + self.assertAlmostEqual(rightClosedInterval.left_value(), 0.) + self.assertFalse(rightClosedInterval.is_left_closed()) + self.assertAlmostEqual(rightClosedInterval.right_value(), 1.) + self.assertTrue(rightClosedInterval.is_right_closed()) + + closedInterval = Intervald.closed(0., 1.) + self.assertAlmostEqual(closedInterval.left_value(), 0.) + self.assertTrue(closedInterval.is_left_closed()) + self.assertAlmostEqual(closedInterval.right_value(), 1.) + self.assertTrue(closedInterval.is_right_closed()) + + def test_empty_interval(self): + self.assertFalse(Intervald.open(0., 1.).empty()) + self.assertTrue(Intervald.open(0., 0.).empty()) + self.assertTrue(Intervald.left_closed(0., 0.).empty()) + self.assertTrue(Intervald.right_closed(0., 0.).empty()) + self.assertFalse(Intervald.closed(0., 0.).empty()) + self.assertTrue(Intervald.closed(1., 0.).empty()) + + def test_interval_membership(self): + emptyInterval = Intervald.open(0., 0.) + self.assertFalse(emptyInterval.contains(0.)) + + openInterval = Intervald.open(0., 1.) + self.assertFalse(openInterval.contains(0.)) + self.assertTrue(openInterval.contains(0.5)) + self.assertFalse(openInterval.contains(1)) + + leftClosedInterval = Intervald.left_closed(0., 1.) + self.assertTrue(leftClosedInterval.contains(0.)) + self.assertTrue(leftClosedInterval.contains(0.5)) + self.assertFalse(leftClosedInterval.contains(1)) + + rightClosedInterval = Intervald.right_closed(0., 1.) + self.assertFalse(rightClosedInterval.contains(0.)) + self.assertTrue(rightClosedInterval.contains(0.5)) + self.assertTrue(rightClosedInterval.contains(1)) + + closedInterval = Intervald.closed(0., 1.) + self.assertTrue(closedInterval.contains(0.)) + self.assertTrue(closedInterval.contains(0.5)) + self.assertTrue(closedInterval.contains(1)) + + degenerateInterval = Intervald.closed(0., 0.) + self.assertTrue(degenerateInterval.contains(0.)) + + def test_interval_subset(self): + openInterval = Intervald.open(0., 1.) + self.assertFalse(openInterval.contains(Intervald.open(0., 0.))) + self.assertFalse(openInterval.contains(Intervald.closed(-1., 0.))) + self.assertFalse(openInterval.contains(Intervald.closed(1., 2.))) + self.assertFalse(openInterval.contains(Intervald.open(0.5, 1.5))) + self.assertFalse(openInterval.contains(Intervald.open(-0.5, 0.5))) + self.assertTrue(openInterval.contains(Intervald.open(0.25, 0.75))) + self.assertFalse(openInterval.contains(Intervald.closed(0., 1.))) + + closedInterval = Intervald.closed(0., 1.) + self.assertFalse(closedInterval.contains(Intervald.open(0., 0.))) + self.assertTrue(closedInterval.contains(Intervald.closed(0., 0.))) + self.assertFalse(closedInterval.contains(Intervald.closed(0.5, 1.5))) + self.assertFalse(closedInterval.contains(Intervald.closed(-0.5, 0.5))) + self.assertTrue(closedInterval.contains(Intervald.closed(0.25, 0.75))) + self.assertTrue(closedInterval.contains(Intervald.open(0., 1.))) + + def test_interval_equality(self): + self.assertNotEqual(Intervald.open(0., 0.), Intervald.open(0., 0.)) + self.assertEqual(Intervald.closed(0., 0.), Intervald.closed(0., 0.)) + self.assertNotEqual(Intervald.open(0., 1.), Intervald.closed(0., 1.)) + self.assertNotEqual( + Intervald.open(0., 1.), Intervald.left_closed(0., 1.)) + self.assertNotEqual( + Intervald.open(0., 1.), Intervald.right_closed(0., 1.)) + self.assertNotEqual( + Intervald.closed(0., 1.), Intervald.left_closed(0., 1.)) + self.assertNotEqual( + Intervald.closed(0., 1.), Intervald.right_closed(0., 1.)) + + def test_interval_intersection(self): + openInterval = Intervald.open(0., 1.) + self.assertFalse(openInterval.intersects(Intervald.open(0.5, 0.5))) + self.assertTrue(openInterval.intersects(Intervald.open(0.5, 1.5))) + self.assertTrue(openInterval.intersects(Intervald.open(-0.5, 0.5))) + self.assertFalse(openInterval.intersects(Intervald.closed(1., 1.))) + self.assertTrue(openInterval.intersects(Intervald.closed(0.5, 0.5))) + self.assertFalse(openInterval.intersects(Intervald.open(1., 2.))) + self.assertFalse(openInterval.intersects(Intervald.open(-1., 0.))) + self.assertFalse(openInterval.intersects(Intervald.left_closed(1., 2.))) + self.assertFalse(openInterval.intersects(Intervald.right_closed(-1., 0.))) + + closedInterval = Intervald.closed(0., 1.) + self.assertFalse(closedInterval.intersects(Intervald.open(1., 1.))) + self.assertTrue(closedInterval.intersects(Intervald.closed(0.5, 1.5))) + self.assertTrue(closedInterval.intersects(Intervald.closed(-0.5, 0.5))) + self.assertFalse(closedInterval.intersects(Intervald.closed(1.5, 2.5))) + self.assertFalse(closedInterval.intersects(Intervald.closed(-1.5, -0.5))) + self.assertFalse(closedInterval.intersects(Intervald.open(1., 2.))) + self.assertFalse(closedInterval.intersects(Intervald.open(-1., 0.))) + self.assertTrue(closedInterval.intersects(Intervald.left_closed(1., 2.))) + self.assertTrue(closedInterval.intersects(Intervald.right_closed(-1., 0.))) + + +if __name__ == '__main__': + unittest.main() From b42117cae97484e72880640f0bb638aced256e21 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 5 Jul 2022 14:48:35 +0200 Subject: [PATCH 8/9] Added Polynomial3 Python interface Signed-off-by: ahcorde --- src/python_pybind11/CMakeLists.txt | 2 + src/python_pybind11/src/Polynomial3.cc | 34 +++ src/python_pybind11/src/Polynomial3.hh | 112 +++++++ .../src/_ignition_math_pybind11.cc | 3 + src/python_pybind11/test/Polynomial3_TEST.py | 275 ++++++++++++++++++ 5 files changed, 426 insertions(+) create mode 100644 src/python_pybind11/src/Polynomial3.cc create mode 100644 src/python_pybind11/src/Polynomial3.hh create mode 100644 src/python_pybind11/test/Polynomial3_TEST.py diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 9729827ac..e073e9295 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -29,6 +29,7 @@ pybind11_add_module(math SHARED src/Matrix4.cc src/MovingWindowFilter.cc src/PID.cc + src/Polynomial3.cc src/Pose3.cc src/Quaternion.cc src/Rand.cc @@ -123,6 +124,7 @@ if (BUILD_TESTING) OrientedBox_TEST PID_TEST Plane_TEST + Polynomial3_TEST Pose3_TEST Quaternion_TEST Rand_TEST diff --git a/src/python_pybind11/src/Polynomial3.cc b/src/python_pybind11/src/Polynomial3.cc new file mode 100644 index 000000000..0b928c923 --- /dev/null +++ b/src/python_pybind11/src/Polynomial3.cc @@ -0,0 +1,34 @@ +/* + * 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 "Polynomial3.hh" + +namespace ignition +{ +namespace math +{ +namespace python +{ +void defineMathPolynomial3(py::module &m, const std::string &typestr) +{ + helpDefineMathPolynomial3(m, typestr + "f"); + helpDefineMathPolynomial3(m, typestr + "d"); +} + +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/Polynomial3.hh b/src/python_pybind11/src/Polynomial3.hh new file mode 100644 index 000000000..7cc09e893 --- /dev/null +++ b/src/python_pybind11/src/Polynomial3.hh @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_MATH_PYTHON__POLYNOMIAL3_HH_ +#define IGNITION_MATH_PYTHON__POLYNOMIAL3_HH_ + +#include +#include + +#include + +#include +#include +#include + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Help define a pybind11 wrapper for an ignition::math::Polynomial3 +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void helpDefineMathPolynomial3(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::Polynomial3; + auto toString = [](const Class &si) { + std::stringstream stream; + stream << si; + return stream.str(); + }; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init>()) + .def("constant", + &Class::Constant, + "Make a constant polynomial") + .def("coeffs", + &Class::Coeffs, + "Get the polynomial coefficients") + .def("evaluate", + &Class::Evaluate, + "Evaluate the polynomial at `_x`. For non-finite `_x`, this function " + "computes p(z) as z tends to `_x`.") + .def("minimum", + [](const Class &self, const Interval &_interval, T &_xMin) + { + T result = self.Minimum(_interval, _xMin); + return std::make_tuple(result, _xMin); + }, + "Compute polynomial minimum in an `_interval`") + .def("minimum", + py::overload_cast &>(&Class::Minimum, py::const_), + "Compute polynomial minimum in an `_interval`") + .def("minimum", + [](const Class &self, T &_xMin) + { + T result = self.Minimum(_xMin); + return std::make_tuple(result, _xMin); + }, + "Compute polynomial minimum in an `_interval`") + .def("minimum", + py::overload_cast<>(&Class::Minimum, py::const_), + "Compute polynomial minimum in an `_interval`") + .def("__call__", &Class::operator()) + .def("__copy__", [](const Class &self) { + return Class(self); + }) + .def("__deepcopy__", [](const Class &self, py::dict) { + return Class(self); + }, "memo"_a) + .def("__str__", toString) + .def("__repr__", toString); +} + +/// Define a pybind11 wrapper for an ignition::math::Polynomial3 +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathPolynomial3(py::module &m, const std::string &typestr); + +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__POLYNOMIAL3_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index 1c9bf494c..a8de23b90 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -39,6 +39,7 @@ #include "OrientedBox.hh" #include "PID.hh" #include "Plane.hh" +#include "Polynomial3.hh" #include "Pose3.hh" #include "Quaternion.hh" #include "Rand.hh" @@ -134,6 +135,8 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathInterval(m, "Interval"); + ignition::math::python::defineMathPolynomial3(m, "Polynomial3"); + ignition::math::python::defineMathLine2(m, "Line2"); ignition::math::python::defineMathLine3(m, "Line3"); diff --git a/src/python_pybind11/test/Polynomial3_TEST.py b/src/python_pybind11/test/Polynomial3_TEST.py new file mode 100644 index 000000000..16e50f2ff --- /dev/null +++ b/src/python_pybind11/test/Polynomial3_TEST.py @@ -0,0 +1,275 @@ +# Copyright (C) 2022 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License") +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +import unittest +from ignition.math import Helpers, Intervald, Polynomial3d, Vector4d, isnan + + +class TestInterval(unittest.TestCase): + + def test_default_constructor(self): + poly = Polynomial3d() + self.assertEqual(poly.coeffs(), Vector4d.ZERO) + + def test_constructor(self): + poly = Polynomial3d(Vector4d.ONE) + self.assertEqual(poly.coeffs(), Vector4d.ONE) + + def test_constructor_helpers(self): + poly = Polynomial3d.constant(1.) + self.assertEqual(poly.coeffs(), Vector4d(0., 0., 0., 1.)) + + def test_evaluate(self): + p = Polynomial3d.constant(1.) + self.assertAlmostEqual(p(-1.), 1.) + self.assertAlmostEqual(p(0.), 1.) + self.assertAlmostEqual(p(1.), 1.) + self.assertAlmostEqual(p(Helpers.INF_D), 1.) + self.assertTrue(isnan(p(Helpers.NAN_D))) + + p = Polynomial3d(Vector4d.ONE) + self.assertAlmostEqual(p(-1.), 0.) + self.assertAlmostEqual(p(0.), 1.) + self.assertAlmostEqual(p(1.), 4.) + self.assertAlmostEqual(p(-Helpers.INF_D), -Helpers.INF_D) + self.assertTrue(isnan(p(Helpers.NAN_D))) + + def test_minimum(self): + p = Polynomial3d.constant(1.) + xInterval = Intervald.open(0., 0.) + xMin = 0. + self.assertTrue(isnan(p.minimum(xInterval))) + min, xMin = p.minimum(xInterval, xMin) + self.assertTrue(isnan(min)) + self.assertTrue(isnan(xMin)) + + p = Polynomial3d.constant(1.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(), 1.) + min, xMin = p.minimum(xMin) + self.assertAlmostEqual(min, 1.) + self.assertFalse(isnan(xMin)) + + p = Polynomial3d(Vector4d(0., 0., 1., 1.)) + xInterval = Intervald.open(0., 1.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 1.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 1.) + self.assertAlmostEqual(xMin, 0.) + + p = Polynomial3d( Vector4d(0., 0., 1., 1.)) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(), -Helpers.INF_D) + min, xMin = p.minimum(xMin) + self.assertAlmostEqual(min, -Helpers.INF_D) + self.assertAlmostEqual(xMin, -Helpers.INF_D) + + p = Polynomial3d( Vector4d(0., 0., -1., 1.)) + xInterval = Intervald.open(0., 1.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 0.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 0.) + self.assertAlmostEqual(xMin, 1.) + + p = Polynomial3d(Vector4d(0., 0., -1., 1.)) + xMin = 0 + self.assertAlmostEqual(p.minimum(), -Helpers.INF_D) + min, xMin = p.minimum(xMin) + self.assertAlmostEqual(min, -Helpers.INF_D) + self.assertAlmostEqual(xMin, Helpers.INF_D) + + p = Polynomial3d( Vector4d(0., 1., 1., 1.)) + xInterval = Intervald.open(1., 2.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 3.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 3.) + self.assertAlmostEqual(xMin, 1.) + + p = Polynomial3d(Vector4d(0., 1., 1., 1.)) + xInterval = Intervald.open(-1., 0.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 0.75) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 0.75) + self.assertAlmostEqual(xMin, -0.5) + + p = Polynomial3d(Vector4d(0., 1., 1., 1.)) + xInterval = Intervald.open(-3., -2.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 3.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 3.) + self.assertAlmostEqual(xMin, -2.) + + p = Polynomial3d(Vector4d(0., 1., 1., 1.)) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(), 0.75) + min, xMin = p.minimum(xMin) + self.assertAlmostEqual(min, 0.75) + self.assertAlmostEqual(xMin, -0.5) + + p = Polynomial3d(Vector4d(0., -1., 1., 1.)) + xInterval = Intervald.open(1., 2.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), -1.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, -1.) + self.assertAlmostEqual(xMin, 2.) + + p = Polynomial3d(Vector4d(0., -1., 1., 1.)) + xInterval = Intervald.open(-2., -1.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), -5.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, -5.) + self.assertAlmostEqual(xMin, -2.) + + p = Polynomial3d(Vector4d(0., -1., 1., 1.)) + xInterval = Intervald.open(0., 1.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 1.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 1.) + self.assertAlmostEqual(xMin, 0.) + + p = Polynomial3d(Vector4d(0., -1., 1., 1.)) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(), -Helpers.INF_D) + min, xMin = p.minimum(xMin) + self.assertAlmostEqual(min, -Helpers.INF_D) + self.assertAlmostEqual(xMin, -Helpers.INF_D) + + p = Polynomial3d(Vector4d(1., 1., 1., 1.)) + xInterval = Intervald.open(-1., 1.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 0.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 0.) + self.assertAlmostEqual(xMin, -1.) + + p = Polynomial3d(Vector4d(1., 1., 1., 1.)) + xInterval =Intervald.open(-2., -1.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), -5.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, -5.) + self.assertAlmostEqual(xMin, -2.) + + p = Polynomial3d(Vector4d(1., 1., 1., 1.)) + xInterval = Intervald.open(2., 3.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 15.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 15.) + self.assertAlmostEqual(xMin, 2.) + + p = Polynomial3d(Vector4d(1., 1., 1., 1.)) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(), -Helpers.INF_D) + min, xMin = p.minimum(xMin) + self.assertAlmostEqual(min, -Helpers.INF_D) + self.assertAlmostEqual(xMin, -Helpers.INF_D) + + p = Polynomial3d(Vector4d(-1., 2., 1., 1.)) + xInterval = Intervald.open(-1., 1.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 0.8873882090776197) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 0.8873882090776197) + self.assertAlmostEqual(xMin, -0.2152504370215302) + + p = Polynomial3d(Vector4d(-1., 2., 1., 1.)) + xInterval = Intervald.open(-3., -2.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 15.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 15.) + self.assertAlmostEqual(xMin, -2.) + + p = Polynomial3d(Vector4d(-1., 2., 1., 1.)) + xInterval = Intervald.open(1., 2.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 3.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 3.) + self.assertAlmostEqual(xMin, 1.) + + p = Polynomial3d(Vector4d(-1., 2., 1., 1.)) + xInterval = Intervald.open(2., 3.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), -5.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, -5.) + self.assertAlmostEqual(xMin, 3.) + + p = Polynomial3d(Vector4d(-1., 2., 1., 1.)) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(), -Helpers.INF_D) + min, xMin = p.minimum(xMin) + self.assertAlmostEqual(min, -Helpers.INF_D) + self.assertAlmostEqual(xMin, Helpers.INF_D) + + p = Polynomial3d(Vector4d(1., -2., 1., 1.)) + xInterval = Intervald.open(-1., 1.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), -3.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, -3.) + self.assertAlmostEqual(xMin, -1.) + + p = Polynomial3d(Vector4d(1., -2., 1., 1.)) + xInterval = Intervald.open(0., 2.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 1.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 1.) + self.assertAlmostEqual(xMin, 0.) + + p = Polynomial3d(Vector4d(1., -2., 1., 1.)) + xInterval = Intervald.open(2., 3.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), 3.) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, 3.) + self.assertAlmostEqual(xMin, 2.) + + p = Polynomial3d(Vector4d(1., -2., 1., 1.)) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(), -Helpers.INF_D) + min, xMin = p.minimum(xMin) + self.assertAlmostEqual(min, -Helpers.INF_D) + self.assertAlmostEqual(xMin, -Helpers.INF_D) + + p = Polynomial3d(Vector4d(1., -4., -2., -1.)) + xInterval = Intervald.open(-1., 6.) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(xInterval), -16.051047904897441) + min, xMin = p.minimum(xInterval, xMin) + self.assertAlmostEqual(min, -16.051047904897441) + self.assertAlmostEqual(xMin, 2.8968052532744766) + self.assertAlmostEqual(p.minimum(), -Helpers.INF_D) + + p = Polynomial3d(Vector4d(1., -4., -2., -1.)) + xMin = Helpers.NAN_D + self.assertAlmostEqual(p.minimum(), -Helpers.INF_D) + min, xMin = p.minimum(xMin) + self.assertAlmostEqual(min, -Helpers.INF_D) + self.assertAlmostEqual(xMin, -Helpers.INF_D) + +if __name__ == '__main__': + unittest.main() From ff22c3b816587d93dda3a71b0e3023d185296363 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 5 Jul 2022 15:07:51 +0200 Subject: [PATCH 9/9] make linters happy Signed-off-by: ahcorde --- src/python_pybind11/src/Polynomial3.hh | 1 - 1 file changed, 1 deletion(-) diff --git a/src/python_pybind11/src/Polynomial3.hh b/src/python_pybind11/src/Polynomial3.hh index 7cc09e893..7b0fede59 100644 --- a/src/python_pybind11/src/Polynomial3.hh +++ b/src/python_pybind11/src/Polynomial3.hh @@ -23,7 +23,6 @@ #include -#include #include #include