From 44d1f180bba5f7886c6aeb6ee3f156d0486cb24b Mon Sep 17 00:00:00 2001 From: Mr-tooth Date: Tue, 17 Mar 2026 17:55:32 +0800 Subject: [PATCH] feat: add convex polygon intersect + contained predicates MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add two new geometric predicates to HeuclidPolygonTools: - isConvexPolygonIntersect(A, B): detect any overlap between two convex polygons (mutual vertex + edge-midpoint containment tests) - isConvexPolygonContained(A, B): check if all vertices of A are inside B (used for terrain footstep placement validation) 16 GTest test cases covering: - Small/large rectangle containment (inside, outside, partial) - Vertex on boundary, rotated polygons (diamond shapes) - Overlapping, disjoint, shared edge, corner touching - Empty polygon edge cases These are generic convex polygon operations — not tied to any specific robotics application — making them reusable for footstep planning, collision detection, and spatial queries. Part of the obstacle avoidance + stair climbing demo work. --- .../geometry/tools/HeuclidPolygonTools.h | 27 +++ .../geometry/tools/HeuclidPolygonTools.cpp | 69 ++++++ src/Test/CMakeLists.txt | 5 + src/Test/TestPolygonPredicate.cpp | 209 ++++++++++++++++++ 4 files changed, 310 insertions(+) create mode 100644 src/Test/TestPolygonPredicate.cpp diff --git a/include/Heuclid/geometry/tools/HeuclidPolygonTools.h b/include/Heuclid/geometry/tools/HeuclidPolygonTools.h index d78def9..9e1d941 100644 --- a/include/Heuclid/geometry/tools/HeuclidPolygonTools.h +++ b/include/Heuclid/geometry/tools/HeuclidPolygonTools.h @@ -55,6 +55,33 @@ class HeuclidGeometryPolygonTools int checkNumberOfVertices(std::vector > Buffer, int _numOfVertices); int checkEdgeOfIndex(int edgeIndex, int _numOfVertices); + + /** + * @brief Check if two convex polygons overlap or intersect. + * + * Uses mutual vertex-in-polygon + edge-midpoint-in-polygon tests + * to detect any geometric overlap between the two convex polygons. + * If no vertex of either polygon is inside the other and no edge + * midpoints overlap, the polygons are disjoint. + * + * @param polyA first convex polygon + * @param polyB second convex polygon + * @return true if the two polygons overlap (intersect), false if disjoint + */ + bool isConvexPolygonIntersect(const ConvexPolygon2D& polyA, const ConvexPolygon2D& polyB); + + /** + * @brief Check if polyA is fully contained within polyB. + * + * For convex polygons, containment is verified by checking that all + * vertices of polyA are inside polyB. This correctly rejects cases where + * polyA straddles polyB's boundary or extends outside polyB. + * + * @param polyA the candidate contained polygon + * @param polyB the containing polygon + * @return true if polyA ⊆ polyB (all vertices of A are inside B) + */ + bool isConvexPolygonContained(const ConvexPolygon2D& polyA, const ConvexPolygon2D& polyB); // int checkNumberOfVertices(const ConvexPolygon2D& convexPolygon2D); // int checkEdgeOfIndex(const ConvexPolygon2D& convexPolygon2D, const int& edgeIndex); diff --git a/src/Heuclid/geometry/tools/HeuclidPolygonTools.cpp b/src/Heuclid/geometry/tools/HeuclidPolygonTools.cpp index acdfcfd..b1c5cac 100644 --- a/src/Heuclid/geometry/tools/HeuclidPolygonTools.cpp +++ b/src/Heuclid/geometry/tools/HeuclidPolygonTools.cpp @@ -166,4 +166,73 @@ int HeuclidGeometryPolygonTools::checkEdgeOfIndex(int edgeIndex, int _numOfVerti return EDAGE_INDEX_OUT; return CHECK_CORRECT; } + +bool HeuclidGeometryPolygonTools::isConvexPolygonContained(const ConvexPolygon2D& polyA, const ConvexPolygon2D& polyB) +{ + const std::vector> verticesA = polyA.getVertexBuffer(); + const std::vector> verticesB = polyB.getVertexBuffer(); + const int nA = polyA.getNumOfVertices(); + const int nB = polyB.getNumOfVertices(); + const bool cwB = polyB.getClockwiseOrder(); + + if(nA == 0 || nB == 0) + return false; + + // All vertices of A must be inside B + for(int i = 0; i < nA; i++) + { + if(!this->isPoint2DInsideConvexPolygon2D(verticesA.at(i).getX(), verticesA.at(i).getY(), verticesB, nB, cwB)) + return false; + } + return true; +} + +bool HeuclidGeometryPolygonTools::isConvexPolygonIntersect(const ConvexPolygon2D& polyA, const ConvexPolygon2D& polyB) +{ + const std::vector> verticesA = polyA.getVertexBuffer(); + const std::vector> verticesB = polyB.getVertexBuffer(); + const int nA = polyA.getNumOfVertices(); + const int nB = polyB.getNumOfVertices(); + const bool cwA = polyA.getClockwiseOrder(); + const bool cwB = polyB.getClockwiseOrder(); + + if(nA == 0 || nB == 0) + return false; + + // Check if any vertex of A is inside B + for(int i = 0; i < nA; i++) + { + if(this->isPoint2DInsideConvexPolygon2D(verticesA.at(i).getX(), verticesA.at(i).getY(), verticesB, nB, cwB)) + return true; + } + + // Check edge midpoints of A against B (catches edge crossing without vertex containment) + for(int i = 0; i < nA; i++) + { + int j = (i + 1) % nA; + double mx = 0.5 * (verticesA.at(i).getX() + verticesA.at(j).getX()); + double my = 0.5 * (verticesA.at(i).getY() + verticesA.at(j).getY()); + if(this->isPoint2DInsideConvexPolygon2D(mx, my, verticesB, nB, cwB)) + return true; + } + + // Check if any vertex of B is inside A + for(int i = 0; i < nB; i++) + { + if(this->isPoint2DInsideConvexPolygon2D(verticesB.at(i).getX(), verticesB.at(i).getY(), verticesA, nA, cwA)) + return true; + } + + // Check edge midpoints of B against A + for(int i = 0; i < nB; i++) + { + int j = (i + 1) % nB; + double mx = 0.5 * (verticesB.at(i).getX() + verticesB.at(j).getX()); + double my = 0.5 * (verticesB.at(i).getY() + verticesB.at(j).getY()); + if(this->isPoint2DInsideConvexPolygon2D(mx, my, verticesA, nA, cwA)) + return true; + } + + return false; +} _LJH_EUCLID_LIB_END \ No newline at end of file diff --git a/src/Test/CMakeLists.txt b/src/Test/CMakeLists.txt index 370f110..2672942 100644 --- a/src/Test/CMakeLists.txt +++ b/src/Test/CMakeLists.txt @@ -28,3 +28,8 @@ gtest_discover_tests(TestConvexHull2D) add_executable(TestBeizer TestBeizer.cpp) target_link_libraries(TestBeizer PRIVATE heuclid GTest::gtest_main) gtest_discover_tests(TestBeizer) + +# Test: Polygon predicates (intersect + contained) +add_executable(TestPolygonPredicate TestPolygonPredicate.cpp) +target_link_libraries(TestPolygonPredicate PRIVATE heuclid GTest::gtest_main) +gtest_discover_tests(TestPolygonPredicate) diff --git a/src/Test/TestPolygonPredicate.cpp b/src/Test/TestPolygonPredicate.cpp new file mode 100644 index 0000000..4ef5e0a --- /dev/null +++ b/src/Test/TestPolygonPredicate.cpp @@ -0,0 +1,209 @@ +// Copyright 2024-2026 Junhang Lai (赖俊杭) +// +// 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 + +using namespace ljh::heuclid; + +// Helper: build a ConvexPolygon2D from a list of points +static ConvexPolygon2D makePoly(const std::vector>& pts, bool cw = true) +{ + ConvexPolygon2D poly((int)pts.size()); + poly.setVertexBuffer(pts); + poly.setClockwiseOrder(cw); + return poly; +} + +// Helper: axis-aligned rectangle +static ConvexPolygon2D makeRect(double x, double y, double w, double h) +{ + // Clockwise: top-left, top-right, bottom-right, bottom-left + std::vector> v; + v.push_back(Point2D(x, y+h)); + v.push_back(Point2D(x+w, y+h)); + v.push_back(Point2D(x+w, y )); + v.push_back(Point2D(x, y )); + return makePoly(v, true); +} + +// ============================================================================ +// isConvexPolygonContained tests +// ============================================================================ + +TEST(ConvexPolygonContained, SmallRectInsideLargeRect) +{ + HeuclidGeometryPolygonTools tools; + auto large = makeRect(0, 0, 10, 10); + auto small = makeRect(2, 2, 3, 3); + EXPECT_TRUE(tools.isConvexPolygonContained(small, large)); +} + +TEST(ConvexPolygonContained, IdenticalRectangles) +{ + HeuclidGeometryPolygonTools tools; + auto a = makeRect(0, 0, 5, 5); + auto b = makeRect(0, 0, 5, 5); + EXPECT_TRUE(tools.isConvexPolygonContained(a, b)); +} + +TEST(ConvexPolygonContained, VertexOnBoundary) +{ + HeuclidGeometryPolygonTools tools; + auto outer = makeRect(0, 0, 10, 10); + // Inner rectangle with one vertex touching the outer's edge + auto touching = makeRect(5, 0, 5, 3); // top-right at (10,3), top-left at (5,3) + // All vertices of touching are inside or on boundary of outer + EXPECT_TRUE(tools.isConvexPolygonContained(touching, outer)); +} + +TEST(ConvexPolygonContained, OutsidePartially) +{ + HeuclidGeometryPolygonTools tools; + auto large = makeRect(0, 0, 10, 10); + auto outside = makeRect(8, 8, 5, 5); // extends beyond large + EXPECT_FALSE(tools.isConvexPolygonContained(outside, large)); +} + +TEST(ConvexPolygonContained, FullyOutside) +{ + HeuclidGeometryPolygonTools tools; + auto a = makeRect(0, 0, 2, 2); + auto b = makeRect(10, 10, 2, 2); + EXPECT_FALSE(tools.isConvexPolygonContained(a, b)); +} + +TEST(ConvexPolygonContained, RotatedSmallSquare) +{ + HeuclidGeometryPolygonTools tools; + auto outer = makeRect(-5, -5, 10, 10); + // Diamond (45-degree rotated square) centered at origin + std::vector> v; + v.push_back(Point2D( 0.0, 2.0)); + v.push_back(Point2D( 2.0, 0.0)); + v.push_back(Point2D( 0.0, -2.0)); + v.push_back(Point2D(-2.0, 0.0)); + auto diamond = makePoly(v, true); + EXPECT_TRUE(tools.isConvexPolygonContained(diamond, outer)); +} + +TEST(ConvexPolygonContained, DiamondPartiallyOutside) +{ + HeuclidGeometryPolygonTools tools; + auto outer = makeRect(-5, -5, 10, 10); + // Diamond centered at corner of outer + std::vector> v; + v.push_back(Point2D( 5.0, 7.0)); // outside + v.push_back(Point2D( 7.0, 5.0)); // outside + v.push_back(Point2D( 5.0, 3.0)); // inside + v.push_back(Point2D( 3.0, 5.0)); // inside + auto diamond = makePoly(v, true); + EXPECT_FALSE(tools.isConvexPolygonContained(diamond, outer)); +} + +// ============================================================================ +// isConvexPolygonIntersect tests +// ============================================================================ + +TEST(ConvexPolygonIntersect, OverlappingRects) +{ + HeuclidGeometryPolygonTools tools; + auto a = makeRect(0, 0, 5, 5); + auto b = makeRect(3, 3, 5, 5); // overlaps at (3,3)-(5,5) + EXPECT_TRUE(tools.isConvexPolygonIntersect(a, b)); +} + +TEST(ConvexPolygonIntersect, DisjointRects) +{ + HeuclidGeometryPolygonTools tools; + auto a = makeRect(0, 0, 2, 2); + auto b = makeRect(5, 5, 2, 2); + EXPECT_FALSE(tools.isConvexPolygonIntersect(a, b)); +} + +TEST(ConvexPolygonIntersect, OneInsideOther) +{ + HeuclidGeometryPolygonTools tools; + auto outer = makeRect(0, 0, 10, 10); + auto inner = makeRect(2, 2, 3, 3); + // Inner is inside outer → they overlap + EXPECT_TRUE(tools.isConvexPolygonIntersect(inner, outer)); + EXPECT_TRUE(tools.isConvexPolygonIntersect(outer, inner)); +} + +TEST(ConvexPolygonIntersect, SharedEdge) +{ + HeuclidGeometryPolygonTools tools; + auto a = makeRect(0, 0, 5, 5); + auto b = makeRect(5, 0, 5, 5); // shared edge at x=5 + // Shared edge → boundary contact → considered intersecting (point on edge returns true) + EXPECT_TRUE(tools.isConvexPolygonIntersect(a, b)); +} + +TEST(ConvexPolygonIntersect, TouchingCorner) +{ + HeuclidGeometryPolygonTools tools; + auto a = makeRect(0, 0, 5, 5); + auto b = makeRect(5, 5, 5, 5); // touch at (5,5) + EXPECT_TRUE(tools.isConvexPolygonIntersect(a, b)); +} + +TEST(ConvexPolygonIntersect, EmptyPolygon) +{ + HeuclidGeometryPolygonTools tools; + auto a = makeRect(0, 0, 5, 5); + ConvexPolygon2D empty(0); + EXPECT_FALSE(tools.isConvexPolygonIntersect(a, empty)); + EXPECT_FALSE(tools.isConvexPolygonIntersect(empty, a)); +} + +TEST(ConvexPolygonIntersect, DiagonalOverlap) +{ + HeuclidGeometryPolygonTools tools; + // Two rectangles overlapping diagonally + auto a = makeRect(0, 0, 4, 4); + auto b = makeRect(2, 2, 4, 4); // overlaps at (2,2)-(4,4) + EXPECT_TRUE(tools.isConvexPolygonIntersect(a, b)); + // Non-overlapping diagonal + auto c = makeRect(4, 4, 4, 4); // just touches corner at (4,4) + EXPECT_TRUE(tools.isConvexPolygonIntersect(a, c)); // corner contact +} + +TEST(ConvexPolygonIntersect, RotatedPolygon) +{ + HeuclidGeometryPolygonTools tools; + auto rect = makeRect(0, 0, 6, 6); + // Diamond inside + std::vector> v; + v.push_back(Point2D( 3.0, 0.0)); + v.push_back(Point2D( 0.0, -3.0)); + v.push_back(Point2D(-3.0, 0.0)); + v.push_back(Point2D( 0.0, 3.0)); + auto diamond = makePoly(v, true); + EXPECT_TRUE(tools.isConvexPolygonIntersect(rect, diamond)); +} + +TEST(ConvexPolygonIntersect, NoOverlapRotated) +{ + HeuclidGeometryPolygonTools tools; + auto rect = makeRect(0, 0, 3, 3); + // Diamond far away + std::vector> v; + v.push_back(Point2D(10.0, 12.0)); + v.push_back(Point2D(12.0, 10.0)); + v.push_back(Point2D(10.0, 8.0)); + v.push_back(Point2D( 8.0, 10.0)); + auto diamond = makePoly(v, true); + EXPECT_FALSE(tools.isConvexPolygonIntersect(rect, diamond)); +}