feat(geofence): add segment geometry helpers (#28758)

* feat(geofence): add segment geometry helpers

Signed-off-by: jonas <jonas.perolini@rigi.tech>

* test(geofence): add test coverage  where a side value is exactly zero

---------

Signed-off-by: jonas <jonas.perolini@rigi.tech>
Co-authored-by: jonas <jonas.perolini@rigi.tech>
This commit is contained in:
Jonas Peroliniandjonas authored and GitHub committed 2026-10-06 15:11:14 -06:00
1 parent 2395526999
commit 63d157859e
3 files changed
+168

No files matched your search

+107
View File
@@ -87,6 +87,113 @@ TEST(GeofenceUtilsTest, SegmentsDisjoint)
EXPECT_EQ(SS::Disjoint, geofence_utils::segmentsIntersect(0, 0, 300, 0, 1000, 1000, 4000, 2000));
}
struct SegmentIntersectionCase {
const char *name;
Vector2d a, b, c, d;
bool intersects;
};
static const SegmentIntersectionCase kSegmentIntersectionCases[] = {
// Contact counts, even when the segments only share an endpoint or overlap along a line.
{"Crossing", {0, 0}, {4, 4}, {0, 4}, {4, 0}, true},
{"NegativeCoordinates", {-4, -4}, {0, 0}, {-4, 0}, {0, -4}, true},
{"SharedEndpoint", {0, 0}, {4, 0}, {4, 0}, {4, 4}, true},
{"EndpointOnInterior", {0, 0}, {4, 0}, {2, 0}, {2, 4}, true},
{"CollinearOverlap", {0, 0}, {4, 0}, {2, 0}, {6, 0}, true},
{"ContainedSegment", {0, 0}, {4, 4}, {1, 1}, {3, 3}, true},
{"IdenticalSegments", {0, 0}, {4, 4}, {0, 0}, {4, 4}, true},
{"CollinearEndpointContact", {0, 0}, {2, 2}, {2, 2}, {4, 4}, true},
// A shared line or overlapping bounding boxes alone do not imply an intersection.
{"HorizontalGap", {0, 0}, {1, 0}, {2, 0}, {3, 0}, false},
{"VerticalGap", {0, 0}, {0, 1}, {0, 2}, {0, 3}, false},
{"SlantedGap", {0, 0}, {1, 1}, {2, 2}, {3, 3}, false},
{"ParallelWithOverlappingBounds", {0, 0}, {4, 4}, {0, 1}, {3, 4}, false},
{"NonparallelWithOverlappingBounds", {0, 0}, {2, 2}, {0, 1}, {1, 3}, false},
// c lies on the line through ab but past b, so only the second straddle test rejects it.
{"EndpointOnLineBeyondSegment", {0, 0}, {4, 0}, {5, 0}, {3, 2}, false},
// Equal endpoints turn a segment into a single point.
{"PointOnInterior", {2, 2}, {2, 2}, {0, 0}, {4, 4}, true},
{"PointOnEndpoint", {0, 0}, {0, 0}, {0, 0}, {4, 4}, true},
{"PointBeyondEndpoint", {5, 5}, {5, 5}, {0, 0}, {4, 4}, false},
{"PointOffLineWithinBounds", {1, 2}, {1, 2}, {0, 0}, {4, 4}, false},
{"CoincidentPoints", {1, 2}, {1, 2}, {1, 2}, {1, 2}, true},
{"DistinctPoints", {1, 2}, {1, 2}, {2, 1}, {2, 1}, false},
// At this latitude, 0.0000001 degrees is about 1 cm and is lost with float coordinates.
{"LatLonCentimetreCrossing", {47.0, 8.0}, {47.0000002, 8.0}, {47.0000001, 7.9999999}, {47.0000001, 8.0000001}, true},
{"LatLonCentimetreGap", {47.0, 8.0}, {47.0000001, 8.0}, {47.0000002, 8.0}, {47.0000003, 8.0}, false},
// A shallow crossing over about 76 km, with roughly 1 cm offsets at either end.
{"LatLonShallowCrossing", {47.0, 8.0}, {47.0, 9.0}, {46.9999999, 8.0}, {47.0000001, 9.0}, true},
};
class GeofenceSegmentIntersectionTest : public ::testing::TestWithParam<SegmentIntersectionCase>
{
};
TEST_P(GeofenceSegmentIntersectionTest, SegmentsIntersectInclusive)
{
const auto &test = GetParam();
// Reversing endpoints or swapping the segments must not change the answer.
for (bool reverse_ab : {false, true}) {
for (bool reverse_cd : {false, true}) {
const auto &a = reverse_ab ? test.b : test.a;
const auto &b = reverse_ab ? test.a : test.b;
const auto &c = reverse_cd ? test.d : test.c;
const auto &d = reverse_cd ? test.c : test.d;
EXPECT_EQ(test.intersects, geofence_utils::segmentsIntersectInclusive(a, b, c, d));
EXPECT_EQ(test.intersects, geofence_utils::segmentsIntersectInclusive(c, d, a, b));
}
}
}
INSTANTIATE_TEST_SUITE_P(GeofenceUtils, GeofenceSegmentIntersectionTest,
::testing::ValuesIn(kSegmentIntersectionCases),
[](const ::testing::TestParamInfo<SegmentIntersectionCase> &test_info)
{
return test_info.param.name;
});
struct PointSegmentDistanceCase {
const char *name;
Vector2d point, a, b;
double distance_squared;
};
static const PointSegmentDistanceCase kPointSegmentDistanceCases[] = {
// A horizontal segment makes the nearest endpoint and perpendicular distances easy to check.
{"BeforeStart", {-2, 3}, {0, 0}, {4, 0}, 13},
{"AfterEnd", {6, 3}, {0, 0}, {4, 0}, 13},
{"AboveStart", {0, 3}, {0, 0}, {4, 0}, 9},
{"AboveInterior", {2, 3}, {0, 0}, {4, 0}, 9},
{"OnSegment", {2, 0}, {0, 0}, {4, 0}, 0},
{"TranslatedSegment", {12, 13}, {10, 10}, {14, 10}, 9},
{"VerticalSegment", {-1, -2}, {-4, -4}, {-4, 0}, 9},
{"DiagonalSegment", {2, 0}, {0, 0}, {2, 2}, 2},
{"ZeroLength", {3, 4}, {1, 1}, {1, 1}, 13},
{"CoincidentPoint", {1, 1}, {1, 1}, {1, 1}, 0},
// Both segments lie on 3x + 4y = 250, exactly 50 m from the origin.
{"CircleTangent", {0, 0}, {-30, 85}, {86, -2}, 2500},
// Over 10 km, subtracting projected squared distances can round this tangent outside the circle.
{"LongCircleTangent", {0, 0}, {-8030, 6085}, {318, -176}, 2500},
};
class GeofencePointSegmentDistanceTest : public ::testing::TestWithParam<PointSegmentDistanceCase> {};
TEST_P(GeofencePointSegmentDistanceTest, SquaredDistance)
{
const auto &test = GetParam();
EXPECT_DOUBLE_EQ(test.distance_squared, geofence_utils::pointToSegmentDistanceSquared(test.point, test.a, test.b));
EXPECT_DOUBLE_EQ(test.distance_squared, geofence_utils::pointToSegmentDistanceSquared(test.point, test.b, test.a));
}
INSTANTIATE_TEST_SUITE_P(GeofenceUtils, GeofencePointSegmentDistanceTest,
::testing::ValuesIn(kPointSegmentDistanceCases),
[](const ::testing::TestParamInfo<PointSegmentDistanceCase> &test_info)
{
return test_info.param.name;
});
TEST(GeofenceUtilsTest, PolygonIsCCW)
{
const Vector2f square[4] = {{0.f, 0.f}, {1.f, 0.f}, {1.f, 1.f}, {0.f, 1.f}};
+46
View File
@@ -33,6 +33,7 @@
#include "geofence_utils.h"
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
namespace geofence_utils
{
@@ -553,6 +554,51 @@ float PlannerPolygons::edgeCost(int a, int b) const
}
bool segmentsIntersectInclusive(const matrix::Vector2d &a, const matrix::Vector2d &b,
const matrix::Vector2d &c, const matrix::Vector2d &d)
{
// Reject separated bounds, including gaps between collinear segments.
for (int axis = 0; axis < 2; ++axis) {
if (math::max(a(axis), b(axis)) < math::min(c(axis), d(axis))
|| math::max(c(axis), d(axis)) < math::min(a(axis), b(axis))) {
return false;
}
}
const matrix::Vector2d ab = b - a;
const matrix::Vector2d cd = d - c;
const double side_c = ab.cross(c - a);
const double side_d = ab.cross(d - a);
const double side_a = cd.cross(a - c);
const double side_b = cd.cross(b - c);
// Each segment must cross or touch the line through the other segment.
return ((side_c <= 0.0 && side_d >= 0.0) || (side_c >= 0.0 && side_d <= 0.0))
&& ((side_a <= 0.0 && side_b >= 0.0) || (side_a >= 0.0 && side_b <= 0.0));
}
double pointToSegmentDistanceSquared(const matrix::Vector2d &point,
const matrix::Vector2d &a, const matrix::Vector2d &b)
{
const matrix::Vector2d direction = b - a;
const matrix::Vector2d to_point = point - a;
const double length_squared = direction.norm_squared();
// along / length_squared locates the projection: 0 at a, 1 at b.
const double along = to_point.dot(direction);
if (along <= 0.0) {
return to_point.norm_squared();
} else if (along >= length_squared) {
return (point - b).norm_squared();
}
// Squared distance to the line: |to_point x direction|^2 / |direction|^2.
// https://mathworld.wolfram.com/2-DimensionalPoint-LineDistance.html
const double cross = to_point.cross(direction);
return cross * cross / length_squared;
}
bool isPolygonCCW(const matrix::Vector2f *vertices, int num_vertices)
{
// https://en.wikipedia.org/wiki/Shoelace_formula
+15
View File
@@ -163,6 +163,21 @@ inline SegSegResult segmentsIntersect(int32_t ax, int32_t ay, int32_t bx, int32_
return SegSegResult::Disjoint;
}
/**
* Return true if segments ab and cd share any point, including endpoints and overlap.
* Zero-length segments are treated as points. Coordinates must be finite and in the same plane.
* For latitude/longitude coordinates, longitudes must use one continuous interval.
*/
bool segmentsIntersectInclusive(const matrix::Vector2d &a, const matrix::Vector2d &b,
const matrix::Vector2d &c, const matrix::Vector2d &d);
/**
* Squared distance from a point to the closest point on segment ab, including its endpoints.
* Use finite Cartesian coordinates in the same units. A zero-length segment is treated as a point.
*/
double pointToSegmentDistanceSquared(const matrix::Vector2d &point,
const matrix::Vector2d &a, const matrix::Vector2d &b);
/**
* Check if a polygon's vertices are ordered counter-clockwise using the shoelace formula.
* Works in local Cartesian coordinates. Returns false for CW or zero-area polygons.