diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b9fea6919..8b0c07da2 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -48,6 +48,9 @@ target_link_libraries(quaternion_to_euler ignition-math${IGN_MATH_VER}::ignition 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(temperature_example temperature_example.cc) target_link_libraries(temperature_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]"); + } +}