mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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:
1 parent
2395526999
commit
63d157859e
3 files changed
+168
No files matched your search
@@ -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}};
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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.
|
||||
|
||||
Reference in new issue
Block a user