From dc660144ac3d4911fd690f34adee4a71d75123ed Mon Sep 17 00:00:00 2001 From: Franco Cipollone Date: Fri, 5 Aug 2022 16:40:47 -0300 Subject: [PATCH] Brings BoundingRegion's related stuff from maliput_object. Signed-off-by: Franco Cipollone --- include/maliput/math/bounding_box.h | 102 +++++++ include/maliput/math/bounding_region.h | 75 +++++ include/maliput/math/overlapping_type.h | 63 +++++ include/maliput/test_utilities/mock_math.h | 53 ++++ src/math/CMakeLists.txt | 2 + src/math/bounding_box.cc | 155 +++++++++++ src/math/overlapping_type.cc | 43 +++ test/math/CMakeLists.txt | 7 + test/math/bounding_box_test.cc | 310 +++++++++++++++++++++ test/math/bounding_region_test.cc | 69 +++++ test/math/overlapping_type_test.cc | 48 ++++ 11 files changed, 927 insertions(+) create mode 100644 include/maliput/math/bounding_box.h create mode 100644 include/maliput/math/bounding_region.h create mode 100644 include/maliput/math/overlapping_type.h create mode 100644 include/maliput/test_utilities/mock_math.h create mode 100644 src/math/bounding_box.cc create mode 100644 src/math/overlapping_type.cc create mode 100644 test/math/bounding_box_test.cc create mode 100644 test/math/bounding_region_test.cc create mode 100644 test/math/overlapping_type_test.cc diff --git a/include/maliput/math/bounding_box.h b/include/maliput/math/bounding_box.h new file mode 100644 index 00000000..54f4d1f3 --- /dev/null +++ b/include/maliput/math/bounding_box.h @@ -0,0 +1,102 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include + +#include "maliput/common/maliput_copyable.h" +#include "maliput/math/bounding_region.h" +#include "maliput/math/overlapping_type.h" +#include "maliput/math/roll_pitch_yaw.h" +#include "maliput/math/vector.h" + +namespace maliput { +namespace math { + +/// Implements BoundingRegion abstract class for non-axis-aligned-box-shaped bounding regions. +class BoundingBox : public BoundingRegion { + public: + MALIPUT_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(BoundingBox) + + /// Constructs a BoundingBox object. + /// The box is defined by a position, dimensions(length, width and height) and orientation. + /// @param position Position of the bounding box in the Inertial-frame. The position matches with the centroid of the + /// box. + /// @param box_size The size of the bounding box on XYZ (length/width,height) + /// @param orientation Orientation of the box in the Inertial-frame. + /// @param tolerance Used to compute IsBoxContained() and IsBoxIntersected() against other BoundingBoxes. + /// @throws maliput::common::assertion_error When tolerance or any box_size's component are negative. + BoundingBox(const Vector3& position, const Vector3& box_size, const RollPitchYaw& orientation, double tolerance); + + ~BoundingBox() = default; + + /// @returns The vertices of the bounding box in the Inertial-frame. + std::vector get_vertices() const; + + /// @returns The orientation of the box in the Inertial-frame. + const RollPitchYaw& get_orientation() const; + + /// @returns The size of the box in length, width and height. + const Vector3& box_size() const; + + /// @returns True when this region contains @p other . + bool IsBoxContained(const BoundingBox& other) const; + + /// @returns True when this region intersects @p other . + bool IsBoxIntersected(const BoundingBox& other) const; + + private: + /// Implements BoundingRegion::do_position() method. + /// @returns Position of the box. + const Vector3& do_position() const override; + + /// Implements BoundingRegion::DoContains() method. + /// @param position Inertial-frame's coordinate. + /// @returns True when @p position is contained in this bounding region. + bool DoContains(const Vector3& position) const override; + + /// Implements BoundingRegion::DoOverlaps() method. + /// Valid @p other 's implementations: + /// - BoundingBox + /// @param other Another bounding region. + /// @returns The overlapping type. + OverlappingType DoOverlaps(const BoundingRegion& other) const override; + + Vector3 position_; + Vector3 box_size_; + RollPitchYaw orientation_; + double tolerance_{}; + + // Half sized box dimensions. + Vector3 xyz_2_; +}; + +} // namespace math +} // namespace maliput diff --git a/include/maliput/math/bounding_region.h b/include/maliput/math/bounding_region.h new file mode 100644 index 00000000..9fd7df7e --- /dev/null +++ b/include/maliput/math/bounding_region.h @@ -0,0 +1,75 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#include "maliput/common/maliput_copyable.h" +#include "maliput/math/overlapping_type.h" + +namespace maliput { +namespace math { + +/// Abstract API for bounding description. +/// @tparam Coordinate Coordinate in a given coordinate system. +template +class BoundingRegion { + public: + MALIPUT_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(BoundingRegion) + + virtual ~BoundingRegion() = default; + + /// Obtains the bounding region's position in the Inertial-frame. + /// The position is expected to match the centroid of the bounding region. + /// @returns The position coordinate. + const Coordinate& position() const { return do_position(); } + + /// Determines whether a @p position in the Inertial-frame is contained in this bounding region. + /// @param position Inertial-frame's coordinate. + /// @returns True when @p position is contained in this bounding region. + bool Contains(const Coordinate& position) const { return DoContains(position); } + + /// Determines the overlapping type with @p other BoundingRegion instance. + /// - OverlappingType::kDisjointed is returned when there is no overlapping with @p other . + /// - OverlappingType::kIntersected is returned when @p other intersects with this region. + /// - OverlappingType::kContained is returned when @p other is contained within this region. + /// @param other Another BoundingRegion. + /// @returns The overlapping type. + OverlappingType Overlaps(const BoundingRegion& other) const { return DoOverlaps(other); } + + protected: + BoundingRegion() = default; + + private: + virtual const Coordinate& do_position() const = 0; + virtual bool DoContains(const Coordinate& position) const = 0; + virtual OverlappingType DoOverlaps(const BoundingRegion& other) const = 0; +}; + +} // namespace math +} // namespace maliput diff --git a/include/maliput/math/overlapping_type.h b/include/maliput/math/overlapping_type.h new file mode 100644 index 00000000..db300ae6 --- /dev/null +++ b/include/maliput/math/overlapping_type.h @@ -0,0 +1,63 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +namespace maliput { +namespace math { + +/// Holds the possible overlapping types between regions. +/// +/// Given two sets `A` and `B` : +/// - `A` intersects `B` iff `A` and `B` have at least one point in common. +/// - `A` contains `B` iff `A` contains all the points of `B`. +/// - `A` disjoints `B` iff `A` and `B` have no points in common. +/// +/// - Example of use: +/// @code {.cpp} +/// OverlappingType MyMethod(); +/// ... +/// if(OverlappingType::kIntersected & MyMethod() == OverlappingType::kIntersected) { +/// // Do something. +/// } +/// @endcode +enum class OverlappingType : unsigned int { + kDisjointed = 0, ///< No overlapping between bounding regions + kIntersected = 1, ///< Bounding regions are intersected. + kContained = 3, ///< Bounding regions are contained. +}; + +// Union operator. +OverlappingType operator|(const OverlappingType& first, const OverlappingType& second); + +// Intersection operator. +OverlappingType operator&(const OverlappingType& first, const OverlappingType& second); + +} // namespace math +} // namespace maliput diff --git a/include/maliput/test_utilities/mock_math.h b/include/maliput/test_utilities/mock_math.h new file mode 100644 index 00000000..c02b4b58 --- /dev/null +++ b/include/maliput/test_utilities/mock_math.h @@ -0,0 +1,53 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include +#include +#include + +#include +#include + +#include "maliput/math/bounding_box.h" +#include "maliput/math/overlapping_type.h" + +namespace maliput { +namespace math { +namespace test { + +class MockBoundingRegion : public BoundingRegion { + public: + MOCK_METHOD((const Vector3&), do_position, (), (const, override)); + MOCK_METHOD((bool), DoContains, (const Vector3&), (const, override)); + MOCK_METHOD((OverlappingType), DoOverlaps, (const BoundingRegion&), (const, override)); +}; + +} // namespace test +} // namespace math +} // namespace maliput diff --git a/src/math/CMakeLists.txt b/src/math/CMakeLists.txt index 27005f2f..e4200d6d 100644 --- a/src/math/CMakeLists.txt +++ b/src/math/CMakeLists.txt @@ -3,8 +3,10 @@ ############################################################################## set(MATH_SOURCES + bounding_box.cc matrix.cc quaternion.cc + overlapping_type.cc roll_pitch_yaw.cc saturate.cc vector.cc diff --git a/src/math/bounding_box.cc b/src/math/bounding_box.cc new file mode 100644 index 00000000..178b004e --- /dev/null +++ b/src/math/bounding_box.cc @@ -0,0 +1,155 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "maliput/math/bounding_box.h" + +namespace maliput { +namespace math { + +BoundingBox::BoundingBox(const Vector3& position, const Vector3& box_size, const RollPitchYaw& orientation, + double tolerance) + : position_(position), + box_size_(box_size), + orientation_(orientation), + tolerance_(tolerance), + xyz_2_(box_size.x() / 2., box_size.y() / 2., box_size.z() / 2.) { + MALIPUT_THROW_UNLESS(tolerance >= 0.); + MALIPUT_THROW_UNLESS(box_size.x() >= 0.); + MALIPUT_THROW_UNLESS(box_size.y() >= 0.); + MALIPUT_THROW_UNLESS(box_size.z() >= 0.); +} + +const Vector3& BoundingBox::do_position() const { return position_; } + +std::vector BoundingBox::get_vertices() const { + const std::vector vertices_box_frame{ + {xyz_2_.x(), xyz_2_.y(), xyz_2_.z()}, {-xyz_2_.x(), xyz_2_.y(), xyz_2_.z()}, + {xyz_2_.x(), -xyz_2_.y(), xyz_2_.z()}, {xyz_2_.x(), xyz_2_.y(), -xyz_2_.z()}, + {-xyz_2_.x(), -xyz_2_.y(), xyz_2_.z()}, {xyz_2_.x(), -xyz_2_.y(), -xyz_2_.z()}, + {-xyz_2_.x(), xyz_2_.y(), -xyz_2_.z()}, {-xyz_2_.x(), -xyz_2_.y(), -xyz_2_.z()}, + }; + std::vector vertices; + for (const auto& vertex : vertices_box_frame) { + vertices.push_back(orientation_.ToMatrix().inverse() * vertex + position_); + } + return vertices; +} + +const Vector3& BoundingBox::box_size() const { return box_size_; } + +const RollPitchYaw& BoundingBox::get_orientation() const { return orientation_; } + +bool BoundingBox::DoContains(const Vector3& position) const { + const Vector3 box_frame_position = orientation_.ToMatrix() * (position - position_); + return box_frame_position.x() <= xyz_2_.x() + tolerance_ && box_frame_position.x() >= -xyz_2_.x() - tolerance_ && + box_frame_position.y() <= xyz_2_.y() + tolerance_ && box_frame_position.y() >= -xyz_2_.y() - tolerance_ && + box_frame_position.z() <= xyz_2_.z() + tolerance_ && box_frame_position.z() >= -xyz_2_.z() - tolerance_; +} + +OverlappingType BoundingBox::DoOverlaps(const BoundingRegion& other) const { + auto other_box = dynamic_cast(&other); + MALIPUT_VALIDATE(other_box != nullptr, "BoundingRegion's implementations supported: BoundingBox."); + if (IsBoxContained(*other_box)) { + return OverlappingType::kContained; + } + if (IsBoxIntersected(*other_box)) { + return OverlappingType::kIntersected; + } + return OverlappingType::kDisjointed; +} + +bool BoundingBox::IsBoxContained(const BoundingBox& other) const { + const auto vertices = other.get_vertices(); + return std::all_of(vertices.begin(), vertices.end(), [this](const auto& vertex) { return this->DoContains(vertex); }); +} + +bool BoundingBox::IsBoxIntersected(const BoundingBox& other) const { + // The following is based on Drake's implementation of drake::geometry::internal::BoxesOverlap() method. + // See https://github.com/RobotLocomotion/drake/blob/master/geometry/proximity/boxes_overlap.cc + + // Let's compute the relative transformation from this box to the other box. + // For the purposes of streamlining the math below, translation and rotation + // will be named `t` and `r` respectively. + const Vector3 t = other.position() - position(); + const Matrix3 r = get_orientation().ToMatrix().inverse() * other.get_orientation().ToMatrix(); + + // Compute some common subexpressions and add epsilon to counteract + // arithmetic error, e.g. when two edges are parallel. We use the value as + // specified from Gottschalk's OBB robustness tests. + const double kEpsilon = 0.000001; + Matrix3 abs_r = r; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + abs_r[i][j] = std::abs(abs_r[i][j]) + kEpsilon; + } + } + + // First category of cases separating along a's axes. + for (int i = 0; i < 3; ++i) { + // if (std::abs(t[i]) > xyz_2_[i] + other.xyz_2_.dot(abs_r.block<1, 3>(i, 0))) { + const Vector3 abs_r_i = abs_r.col(i); + if (std::abs(t[i]) > xyz_2_[i] + other.xyz_2_.dot(abs_r_i)) { + return false; + } + } + + // Second category of cases separating along b's axes. + for (int i = 0; i < 3; ++i) { + // if (std::abs(t.dot(r.block<3, 1>(0, i))) > + // other.xyz_2_[i] + xyz_2_.dot(abs_r.block<3, 1>(0, i))) { + const Vector3 r_i = r.col(i); + const Vector3 abs_r_i = abs_r.col(i); + if (std::abs(t.dot(r_i)) > other.xyz_2_[i] + xyz_2_.dot(abs_r_i)) { + return false; + } + } + + // Third category of cases separating along the axes formed from the cross + // products of a's and b's axes. + int i1 = 1; + for (int i = 0; i < 3; ++i) { + const int i2 = (i1 + 1) % 3; // Calculate common sub expressions. + int j1 = 1; + for (int j = 0; j < 3; ++j) { + const int j2 = (j1 + 1) % 3; + if (std::abs(t[i2] * r[i1][j] - t[i1] * r[i2][j]) > xyz_2_[i1] * abs_r[i2][j] + xyz_2_[i2] * abs_r[i1][j] + + other.xyz_2_[j1] * abs_r[i][j2] + + other.xyz_2_[j2] * abs_r[i][j1]) { + return false; + } + j1 = j2; + } + i1 = i2; + } + + return true; +} + +} // namespace math +} // namespace maliput diff --git a/src/math/overlapping_type.cc b/src/math/overlapping_type.cc new file mode 100644 index 00000000..cf91fe3d --- /dev/null +++ b/src/math/overlapping_type.cc @@ -0,0 +1,43 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "maliput/math/overlapping_type.h" + +namespace maliput { +namespace math { + +OverlappingType operator|(const OverlappingType& first, const OverlappingType& second) { + return OverlappingType(static_cast(first) | static_cast(second)); +} +OverlappingType operator&(const OverlappingType& first, const OverlappingType& second) { + return OverlappingType(static_cast(first) & static_cast(second)); +} + +} // namespace math +} // namespace maliput diff --git a/test/math/CMakeLists.txt b/test/math/CMakeLists.txt index 0d901ee5..63f94e40 100644 --- a/test/math/CMakeLists.txt +++ b/test/math/CMakeLists.txt @@ -1,4 +1,5 @@ find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_gmock REQUIRED) ament_add_gtest(matrix_test matrix_test.cc) ament_add_gtest(quaternion_test quaternion_test.cc) @@ -6,6 +7,9 @@ ament_add_gtest(roll_pitch_yaw_test roll_pitch_yaw_test.cc) ament_add_gtest(saturate_test saturate_test.cc) ament_add_gtest(vector_test vector_test.cc) ament_add_gtest(kd_tree_test kd_tree_test.cc) +ament_add_gtest(overlapping_type_test overlapping_type_test.cc) +ament_add_gtest(bounding_box_test bounding_box_test.cc) +ament_add_gmock(bounding_region_test bounding_region_test.cc) macro(add_dependencies_to_test target) if (TARGET ${target}) @@ -32,3 +36,6 @@ add_dependencies_to_test(roll_pitch_yaw_test) add_dependencies_to_test(saturate_test) add_dependencies_to_test(vector_test) add_dependencies_to_test(kd_tree_test) +add_dependencies_to_test(overlapping_type_test) +add_dependencies_to_test(bounding_box_test) +add_dependencies_to_test(bounding_region_test) diff --git a/test/math/bounding_box_test.cc b/test/math/bounding_box_test.cc new file mode 100644 index 00000000..6cf7ee70 --- /dev/null +++ b/test/math/bounding_box_test.cc @@ -0,0 +1,310 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "maliput/math/bounding_box.h" + +#include +#include + +#include "maliput/math/overlapping_type.h" + +namespace maliput { +namespace math { +namespace test { +namespace { + +static constexpr double kTolerance{1e-12}; + +TEST(BoundingBox, Constructors) { + // Throws due to a negative tolerance value. + EXPECT_THROW( + BoundingBox({1., 2., 3.} /* position */, {2., 2., 2.} /* box_size */, {0., 0., 0.} /* rpy */, -1 /* tolerance */), + maliput::common::assertion_error); + // It doesn't throw. + EXPECT_NO_THROW(BoundingBox({1., 2., 3.} /* position */, {2., 2., 2.} /* box_size */, {0., 0., 0.} /* rpy */, + 1e-12 /* tolerance */);); +} + +struct ExpectedPositionContainsResults { + Vector3 position{}; + bool contains_position{}; +}; + +struct BoundingBoxCase { + Vector3 position{}; + Vector3 box_size{}; + RollPitchYaw rpy{}; + + std::vector expected_position_contains_results{}; + std::vector expected_vertices_result{}; +}; + +std::vector GetTestParameters() { + return { + // Centered at origin, no rotation. + BoundingBoxCase{ + {0., 0., 0.} /* position */, + {2., 2., 2.} /* box_size */, + {0., 0., 0.} /* rpy */, + { + {{1., 1., 1}, true}, /* Right in a vertex */ + {{-1., 1., 1}, true}, /* Right in a vertex */ + {{1., -1., 1}, true}, /* Right in a vertex */ + {{1., 1., -1}, true}, /* Right in a vertex */ + {{2., 1., 1.}, false}, /*A bit of on x*/ + {{1., 2., 1.}, false}, /*A bit of on y*/ + {{1., 1., 2.}, false}, /*A bit of on z*/ + }, + {{1., 1., 1.}, + {-1., 1., 1.}, + {1., -1., 1.}, + {1., 1., -1.}, + {-1., -1., 1.}, + {-1., 1., -1.}, + {1., -1., -1.}, + {-1., -1., -1.}} /* vertices */ + }, + // Centered away from origin, no rotation. + BoundingBoxCase{ + {10., 10., 10.} /* position */, + {2., 2., 2.} /* box_size */, + {0., 0., 0.} /* rpy */, + { + {{11., 11., 11}, true}, /* Right in a vertex */ + {{9., 11., 11}, true}, /* Right in a vertex */ + {{11., 9., 11}, true}, /* Right in a vertex */ + {{11., 11., 9}, true}, /* Right in a vertex */ + {{12., 11., 11.}, false}, /*A bit of on x*/ + {{11., 12., 11.}, false}, /*A bit of on y*/ + {{11., 11., 12.}, false}, /*A bit of on z*/ + }, + {{11., 11., 11.}, + {9., 11., 11.}, + {11., 9., 11.}, + {11., 11., 9.}, + {9., 9., 11.}, + {9., 11., 9.}, + {11., 9., 9.}, + {9., 9., 9.}} /* vertices */ + }, + // Centered at origin, 45 degree yaw rotation. + BoundingBoxCase{ + {0., 0., 0.} /* position */, + {2., 2., 2.} /* box_size */, + {0., 0., M_PI_4} /* rpy */, + { + {{sqrt(2.), 0., 1}, true}, /* Right in a vertex */ + {{-sqrt(2.), 0., 1}, true}, /* Right in a vertex */ + {{sqrt(2.), 0., -1}, true}, /* Right in a vertex */ + {{-sqrt(2.), 0., -1}, true}, /* Right in a vertex */ + {{0., sqrt(2.), 1}, true}, /* Right in a vertex */ + {{0., -sqrt(2.), 1}, true}, /* Right in a vertex */ + {{0., sqrt(2.), -1}, true}, /* Right in a vertex */ + {{0., -sqrt(2.), -1}, true}, /* Right in a vertex */ + {{1., 1., 1}, false}, /* Right in a non-rotated */ + {{-1., 1., 1}, false}, /* Right in a non-rotated */ + {{1., -1., 1}, false}, /* Right in a non-rotated */ + {{1., 1., -1}, false}, /* Right in a non-rotated */ + }, + {{sqrt(2.), 0., 1}, + {-sqrt(2.), 0., 1}, + {sqrt(2.), 0., -1}, + {-sqrt(2.), 0., -1}, + {0., sqrt(2.), 1}, + {0., -sqrt(2.), 1}, + {0., sqrt(2.), -1}, + {0., -sqrt(2.), -1}} /* vertices */ + }, + // Centered at origin, 45 degree pitch rotation. + BoundingBoxCase{ + {0., 0., 0.} /* position */, + {2., 2., 2.} /* box_size */, + {0., M_PI_4, 0.} /* rpy */, + { + {{0., 1., sqrt(2.)}, true}, /* Right in a vertex */ + {{0., 1., -sqrt(2.)}, true}, /* Right in a vertex */ + {{0., -1., sqrt(2.)}, true}, /* Right in a vertex */ + {{0., -1., -sqrt(2.)}, true}, /* Right in a vertex */ + {{sqrt(2.), 1., 0.}, true}, /* Right in a vertex */ + {{sqrt(2.), -1., 0.}, true}, /* Right in a vertex */ + {{-sqrt(2.), 1., 0.}, true}, /* Right in a vertex */ + {{-sqrt(2.), -1., 0.}, true}, /* Right in a vertex */ + {{1., 1., 1}, false}, /* Right in a non-rotated */ + {{-1., 1., 1}, false}, /* Right in a non-rotated */ + {{1., -1., 1}, false}, /* Right in a non-rotated */ + {{1., 1., -1}, false}, /* Right in a non-rotated */ + }, + { + {0., 1., sqrt(2.)}, + {0., 1., -sqrt(2.)}, + {0., -1., sqrt(2.)}, + {0., -1., -sqrt(2.)}, + {sqrt(2.), 1., 0.}, + {sqrt(2.), -1., 0.}, + {-sqrt(2.), 1., 0.}, + {-sqrt(2.), -1., 0.}, + } /* vertices */ + }, + // Centered at origin, 45 degree roll rotation. + BoundingBoxCase{ + {0., 0., 0.} /* position */, + {2., 2., 2.} /* box_size */, + {M_PI_4, 0., 0.} /* rpy */, + { + {{1., 0., sqrt(2.)}, true}, /* Right in a vertex */ + {{-1., 0., sqrt(2.)}, true}, /* Right in a vertex */ + {{1., 0., -sqrt(2.)}, true}, /* Right in a vertex */ + {{-1., 0., -sqrt(2.)}, true}, /* Right in a vertex */ + {{1., sqrt(2.), 0.}, true}, /* Right in a vertex */ + {{1., -sqrt(2.), 0.}, true}, /* Right in a vertex */ + {{-1., sqrt(2.), 0.}, true}, /* Right in a vertex */ + {{-1., -sqrt(2.), 0.}, true}, /* Right in a vertex */ + {{1., 1., 1}, false}, /* Right in a non-rotated */ + {{-1., 1., 1}, false}, /* Right in non-rotated */ + {{1., -1., 1}, false}, /* Right in non-rotated */ + {{1., 1., -1}, false}, /* Right in non-rotated */ + }, + { + {1., 0., sqrt(2.)}, + {-1., 0., sqrt(2.)}, + {1., 0., -sqrt(2.)}, + {-1., 0., -sqrt(2.)}, + {1., sqrt(2.), 0.}, + {1., -sqrt(2.), 0.}, + {-1., sqrt(2.), 0.}, + {-1., -sqrt(2.), 0.}, + } /* vertices */ + }, + }; +} + +class BoundingBoxTest : public ::testing::TestWithParam { + public: + void SetUp() override {} + BoundingBoxCase case_ = GetParam(); +}; + +TEST_P(BoundingBoxTest, ContainsPosition) { + BoundingBox dut{case_.position, case_.box_size, case_.rpy, kTolerance}; + + for (const auto expected_result : case_.expected_position_contains_results) { + EXPECT_EQ(expected_result.contains_position, dut.Contains(expected_result.position)) + << "Expected contains value: " + std::string(expected_result.contains_position ? "True" : "False") + + " at position: " + expected_result.position.to_str(); + } +} + +TEST_P(BoundingBoxTest, GetVertices) { + BoundingBox dut{case_.position, case_.box_size, case_.rpy, kTolerance}; + const auto vertices = dut.get_vertices(); + for (const auto& vertex : vertices) { + const auto expected_vertex_itr = + std::find_if(case_.expected_vertices_result.begin(), case_.expected_vertices_result.end(), + [&vertex](const auto& expected_vertex) { + return std::abs(vertex.x() - expected_vertex.x()) < kTolerance && + std::abs(vertex.y() - expected_vertex.y()) < kTolerance && + std::abs(vertex.z() - expected_vertex.z()) < kTolerance; + }); + EXPECT_NE(expected_vertex_itr, case_.expected_vertices_result.end()) + << "Vertex: " << vertex.to_str() << " doesn't match an expected vertex."; + } +} + +TEST_P(BoundingBoxTest, GetBoxSize) { + BoundingBox dut{case_.position, case_.box_size, case_.rpy, kTolerance}; + EXPECT_EQ(case_.box_size, dut.box_size()); +} + +TEST_P(BoundingBoxTest, GetPosition) { + BoundingBox dut{case_.position, case_.box_size, case_.rpy, kTolerance}; + EXPECT_EQ(case_.position, dut.position()); +} + +TEST_P(BoundingBoxTest, GetOrientation) { + BoundingBox dut{case_.position, case_.box_size, case_.rpy, kTolerance}; + EXPECT_EQ(case_.rpy.vector(), dut.get_orientation().vector()); +} + +INSTANTIATE_TEST_CASE_P(BoundingBoxTestGroup, BoundingBoxTest, ::testing::ValuesIn(GetTestParameters())); + +class BoundingBoxOverlappingTest : public ::testing::Test { + public: + const Vector3 position{1., 2., 3.}; + const Vector3 box_size{4., 4., 4.}; + const RollPitchYaw rpy{M_PI_4, M_PI_4, M_PI_4}; + const BoundingBox dut{position, box_size, rpy, kTolerance}; +}; + +// Tests IsBoxContained method on another BoundingBox. +// This method is implemented using `get_vertices` method and `Contains` method for positions, which were already +// tested. +TEST_F(BoundingBoxOverlappingTest, IsBoxContained) { + // Smaller cube in same location and orientation. -> It contains it. + EXPECT_TRUE(dut.IsBoxContained(BoundingBox(position, {3., 3., 3.}, rpy, kTolerance))); + // Larger cube in same location and orientation. -> It doesn't contain it. + EXPECT_FALSE(dut.IsBoxContained(BoundingBox(position, {5., 5., 5.}, rpy, kTolerance))); + // Same cube in same location and orientation. -> It contains it. + EXPECT_TRUE(dut.IsBoxContained(BoundingBox(position, box_size, rpy, kTolerance))); + // Same cube in different location and same orientation. -> It doesn't contain it. + EXPECT_FALSE(dut.IsBoxContained(BoundingBox({-1, -2., -3.}, box_size, rpy, kTolerance))); + // Same cube in same location and different orientation. -> It doesn't contain it. + EXPECT_FALSE(dut.IsBoxContained(BoundingBox(position, box_size, {0., 0., 0.}, kTolerance))); +} + +// Tests IsBoxIntersected method. The method is based on Drake's implementation of +// drake::geometry::internal::BoxesOverlap() method. See +// https://github.com/RobotLocomotion/drake/blob/master/geometry/proximity/boxes_overlap.cc +TEST_F(BoundingBoxOverlappingTest, IsBoxIntersected) { + // Smaller cube in same location and orientation. -> It contains it so there is intersection. + EXPECT_TRUE(dut.IsBoxIntersected(BoundingBox(position, {3., 3., 3.}, rpy, kTolerance))); + // Larger cube in same location and orientation. -> Intersects. + EXPECT_TRUE(dut.IsBoxIntersected(BoundingBox(position, {5., 5., 5.}, rpy, kTolerance))); + // Same cube in same location and different orientation. -> Intersects. + EXPECT_TRUE(dut.IsBoxIntersected(BoundingBox(position, box_size, {0., 0., 0.}, kTolerance))); + // Same cube in different location. -> It doesn't intersect. + EXPECT_FALSE(dut.IsBoxIntersected(BoundingBox({-1, -2., -3.}, box_size, rpy, kTolerance))); +} + +TEST_F(BoundingBoxOverlappingTest, Overlaps) { + // Tests from BoundingRegion API. + std::unique_ptr> region = std::make_unique(dut); + // Smaller cube in same location and orientation. -> OverlappingType::kContained. + EXPECT_EQ(OverlappingType::kContained, region->Overlaps(BoundingBox(position, {3., 3., 3.}, rpy, kTolerance))); + // Same cube in same location and orientation. -> OverlappingType::kContained. + EXPECT_EQ(OverlappingType::kContained, region->Overlaps(BoundingBox(position, box_size, rpy, kTolerance))); + // Same cube in a different location, no points in common. -> OverlappingType::kDisjointed. + EXPECT_EQ(OverlappingType::kDisjointed, region->Overlaps(BoundingBox({-1, -2., -3.}, box_size, rpy, kTolerance))); + // Same cube in same location and different orientation. -> OverlappingType::kIntersected. + EXPECT_EQ(OverlappingType::kIntersected, region->Overlaps(BoundingBox(position, box_size, {0., 0., 0.}, kTolerance))); +} + +} // namespace +} // namespace test +} // namespace math +} // namespace maliput diff --git a/test/math/bounding_region_test.cc b/test/math/bounding_region_test.cc new file mode 100644 index 00000000..d1c8bf71 --- /dev/null +++ b/test/math/bounding_region_test.cc @@ -0,0 +1,69 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "maliput/math/bounding_region.h" + +#include + +#include + +#include "maliput/math/vector.h" +#include "maliput/test_utilities/mock_math.h" + +namespace maliput { +namespace math { +namespace test { +namespace { + +// Tests API interface. +class BoundingRegionTest : public ::testing::Test { + public: + const bool kExpectedContains{true}; + const Vector3 kExpectedPosition{1., 2., 3.}; + const OverlappingType kExpectedOverlapping{OverlappingType::kContained}; + const std::unique_ptr> dut_ = std::make_unique(); +}; + +TEST_F(BoundingRegionTest, API) { + MockBoundingRegion* mock_region{nullptr}; + ASSERT_NE(nullptr, mock_region = dynamic_cast(dut_.get())); + + EXPECT_CALL(*mock_region, do_position()).Times(1).WillOnce(::testing::ReturnRef(kExpectedPosition)); + EXPECT_CALL(*mock_region, DoContains(Vector3::Ones())).Times(1).WillOnce(::testing::Return(kExpectedContains)); + EXPECT_CALL(*mock_region, DoOverlaps(::testing::_)).Times(1).WillOnce(::testing::Return(kExpectedOverlapping)); + + EXPECT_EQ(kExpectedPosition, dut_->position()); + EXPECT_EQ(kExpectedContains, dut_->Contains(Vector3::Ones())); + EXPECT_EQ(kExpectedOverlapping, dut_->Overlaps(*dut_)); +} + +} // namespace +} // namespace test +} // namespace math +} // namespace maliput diff --git a/test/math/overlapping_type_test.cc b/test/math/overlapping_type_test.cc new file mode 100644 index 00000000..f9d8c392 --- /dev/null +++ b/test/math/overlapping_type_test.cc @@ -0,0 +1,48 @@ +// BSD 3-Clause License +// +// Copyright (c) 2022, Woven Planet. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "maliput/math/overlapping_type.h" + +#include + +namespace maliput { +namespace math { +namespace test { +namespace { + +TEST(OverlappingTypeTest, TestOverlappingType) { + EXPECT_EQ(OverlappingType::kIntersected, OverlappingType::kContained & OverlappingType::kIntersected); + EXPECT_EQ(OverlappingType::kDisjointed, OverlappingType::kContained & OverlappingType::kDisjointed); + EXPECT_EQ(OverlappingType::kContained, (OverlappingType::kContained | OverlappingType::kIntersected)); +} + +} // namespace +} // namespace test +} // namespace math +} // namespace maliput