Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-09 22:08:56 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

geofence_utils: classify vertex hits via wedge, drop sort+sub-segment scan

Browse Source 
Replace the per-sub-segment midpoint scan in lineSegmentIntersectsPolygon
with a wedge classification per vertex hit: a polygon vertex on the open
segment is a real boundary crossing iff the two segment endpoints fall on
different sides of its interior wedge. The insertion sort is gone; the
remaining sample check collapses to start/midpoint/end.

Also update the segmentsIntersect tests for the SegSegResult enum
introduced in the previous commit, and add direct orient2d and
collinear-disjoint cases.
This commit is contained in:
Balduin
2026-05-07 18:45:28 +02:00
parent 6883add606
commit 0a5361dd44
2 changed files with 76 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/geofence/GeofenceUtilsTest.cpp
+34 -18
View File
@@ -37,16 +37,27 @@
using namespace matrix;
using geofence_utils::SegSegResult;
TEST(GeofenceUtilsTest, Orient2d)
{
// CCW turn -> +1, CW turn -> -1, collinear -> 0.
EXPECT_EQ(1, geofence_utils::orient2d({0.f, 0.f}, {1.f, 0.f}, {0.f, 1.f}));
EXPECT_EQ(-1, geofence_utils::orient2d({0.f, 0.f}, {1.f, 0.f}, {0.f, -1.f}));
EXPECT_EQ(0, geofence_utils::orient2d({0.f, 0.f}, {2.f, 2.f}, {1.f, 1.f}));
EXPECT_EQ(0, geofence_utils::orient2d({0.f, 0.f}, {2.f, 2.f}, {3.f, 3.f}));
}
TEST(GeofenceUtilsTest, SegmentsSharedEndpointNoIntersection)
{
// Two segments share endpoint (1,1) but go in different directions — no crossing
// Collinear, second segment is a sub-interval of the first.
Vector2f p1(0.f, 0.f);
Vector2f p2(2.f, 2.f);
Vector2f v1(1.f, 1.f);
Vector2f v2(2.f, 2.f);
EXPECT_FALSE(geofence_utils::segmentsIntersect(p1, p2, v1, v2));
EXPECT_EQ(SegSegResult::CollinearOverlap, geofence_utils::segmentsIntersect(p1, p2, v1, v2));
}
TEST(GeofenceUtilsTest, SegmentsCross)
@@ -58,7 +69,7 @@ TEST(GeofenceUtilsTest, SegmentsCross)
Vector2f v1(-0.0001f, 0.0001f);
Vector2f v2(1.f, 0.0001f);
EXPECT_TRUE(geofence_utils::segmentsIntersect(p1, p2, v1, v2));
EXPECT_EQ(SegSegResult::Proper, geofence_utils::segmentsIntersect(p1, p2, v1, v2));
}
TEST(GeofenceUtilsTest, SegmentsTouching)
@@ -71,37 +82,42 @@ TEST(GeofenceUtilsTest, SegmentsTouching)
Vector2f v1(0.f, 1.0f);
Vector2f v2(1.f, 1.0f);
// This asymmetry may seem very weird. It is explained in segmentsIntersect.
// If vertical line is part of polygon (first arg): No intersection
EXPECT_FALSE(geofence_utils::segmentsIntersect(p1, p2, v1, v2));
// If horizontal line is part of polygon (second arg): intersection
EXPECT_TRUE(geofence_utils::segmentsIntersect(v1, v2, p1, p2));
// Endpoint v1 of segment cd lies on the open segment ab. Symmetric in
// argument order: the convention-laden asymmetry of the old API is gone.
EXPECT_EQ(SegSegResult::Touching, geofence_utils::segmentsIntersect(p1, p2, v1, v2));
EXPECT_EQ(SegSegResult::Touching, geofence_utils::segmentsIntersect(v1, v2, p1, p2));
// Same, but with vertical line slanted for good measure
p1(0) = -1.0f;
p2(0) = 1.0f;
EXPECT_FALSE(geofence_utils::segmentsIntersect(p1, p2, v1, v2));
EXPECT_TRUE(geofence_utils::segmentsIntersect(v1, v2, p1, p2));
EXPECT_EQ(SegSegResult::Touching, geofence_utils::segmentsIntersect(p1, p2, v1, v2));
EXPECT_EQ(SegSegResult::Touching, geofence_utils::segmentsIntersect(v1, v2, p1, p2));
}
TEST(GeofenceUtilsTest, SegmentsParallel)
{
// Slope = 1/3 for both lines
Vector2f p1(0.f, 0.f);
Vector2f p2(3.0f, 0.f);
Vector2f v1(10.f, 10.f);
Vector2f v2(40.f, 20.f);
// Parallel lines never intersect
EXPECT_FALSE(geofence_utils::segmentsIntersect(p1, p2, v1, v2));
// Disjoint, non-collinear: no intersection.
EXPECT_EQ(SegSegResult::None, geofence_utils::segmentsIntersect(p1, p2, v1, v2));
// Equal lines also don't (our convention)
EXPECT_FALSE(geofence_utils::segmentsIntersect(p1, p2, p1, p2));
EXPECT_FALSE(geofence_utils::segmentsIntersect(v1, v2, v1, v2));
// A segment with itself is fully collinear-overlapping.
EXPECT_EQ(SegSegResult::CollinearOverlap, geofence_utils::segmentsIntersect(p1, p2, p1, p2));
EXPECT_EQ(SegSegResult::CollinearOverlap, geofence_utils::segmentsIntersect(v1, v2, v1, v2));
}
TEST(GeofenceUtilsTest, SegmentsCollinearDisjoint)
{
// Two segments on the same line but with non-overlapping intervals.
Vector2f a(0.f, 0.f), b(1.f, 0.f);
Vector2f c(2.f, 0.f), d(3.f, 0.f);
EXPECT_EQ(SegSegResult::None, geofence_utils::segmentsIntersect(a, b, c, d));
}
TEST(GeofenceUtilsTest, SegmentPolygonExclusionOutside)
src/lib/geofence/geofence_utils.cpp
+42 -39
View File
@@ -115,6 +115,24 @@ SegSegResult segmentsIntersect(const matrix::Vector2f &a, const matrix::Vector2f
return SegSegResult::None;
}
// Is point P strictly inside the CCW/CW interior wedge of polygon vertex V,
// where Vp and Vn are the previous and next polygon vertices?
//
// Reference: standard convex/reflex vertex test built from orient2d. P lies
// strictly to the left of both incident edges (Vp->V and V->Vn) for a convex
// vertex, or to the left of either one for a reflex vertex.
static bool pointInsideWedge(const matrix::Vector2f &P,
const matrix::Vector2f &V,
const matrix::Vector2f &Vp,
const matrix::Vector2f &Vn)
{
const int o_in = orient2d(Vp, V, P);
const int o_out = orient2d(V, Vn, P);
const int is_convex = orient2d(Vp, V, Vn);
return is_convex > 0 ? (o_in > 0 && o_out > 0) : (o_in > 0 || o_out > 0);
}
bool lineSegmentIntersectsPolygon(const matrix::Vector2f &start, const matrix::Vector2f &end,
const matrix::Vector2f *vertices, int num_vertices, bool is_inclusion_zone)
{
@@ -131,56 +149,41 @@ bool lineSegmentIntersectsPolygon(const matrix::Vector2f &start, const matrix::V
// Polygon vertices touching the open segment do not by themselves prove a
// crossing -- the segment may merely graze (run along an incident edge,
// or skim a corner from outside). Record those as split points and below
// classify each sub-segment by sampling its midpoint.
const matrix::Vector2f delta = end - start;
const float delta_norm_sq = delta.dot(delta);
// or skim a corner). For each such vertex, classify each segment endpoint
// against the wedge: if start is strictly inside the wedge and end is
// strictly outside (or vice versa), the segment really does cross the
// boundary at V.
for (int i = 0; i < num_vertices; i++) {
if (orient2d(start, end, vertices[i]) != 0 ||
!collinearBetween(start, end, vertices[i]) ||
vertices[i] == start || vertices[i] == end) {
continue;
}
float vertex_hit_params[num_vertices];
int num_hits = 0;
const int prev = (i == 0) ? num_vertices - 1 : i - 1;
const int next = (i + 1) % num_vertices;
if (delta_norm_sq >= FLT_EPSILON) {
for (int i = 0; i < num_vertices; i++) {
if (orient2d(start, end, vertices[i]) != 0) { continue; }
const float s = delta.dot(vertices[i] - start) / delta_norm_sq;
if (s > FLT_EPSILON && s < 1.0f - FLT_EPSILON) {
vertex_hit_params[num_hits++] = s;
}
if (pointInsideWedge(start, vertices[i], vertices[prev], vertices[next]) !=
pointInsideWedge(end, vertices[i], vertices[prev], vertices[next])) {
return true;
}
}
// Insertion sort -- num_hits is small.
for (int i = 1; i < num_hits; i++) {
const float k = vertex_hit_params[i];
int j = i - 1;
// No crossings: the segment lies entirely on one side of the boundary
// (or along it). Sample start, midpoint, end -- one of them will reveal
// the side unless the segment runs exactly along the boundary throughout
// (in which case OnBoundary at all three is the correct, allowed answer).
const matrix::Vector2f samples[3] = {
start, 0.5f * (start + end), end
};
while (j >= 0 && vertex_hit_params[j] > k) {
vertex_hit_params[j + 1] = vertex_hit_params[j];
j--;
}
vertex_hit_params[j + 1] = k;
}
// Each sub-segment between consecutive split points lies wholly on one
// side of the polygon boundary (or along it). Sample the midpoint.
// OnBoundary is treated as allowed for both zone types -- a segment
// running exactly along the fence line is not a violation.
float prev_s = 0.0f;
for (int i = 0; i <= num_hits; i++) {
const float next_s = (i == num_hits) ? 1.0f : vertex_hit_params[i];
const matrix::Vector2f sample = start + (0.5f * (prev_s + next_s)) * delta;
const PointPolygonRelation r = classifyPointInPolygon(vertices, num_vertices, sample);
for (const matrix::Vector2f &p : samples) {
const PointPolygonRelation r = classifyPointInPolygon(vertices, num_vertices, p);
if ((is_inclusion_zone && r == PointPolygonRelation::Outside) ||
(!is_inclusion_zone && r == PointPolygonRelation::Inside)) {
return true;
}
prev_s = next_s;
}
return false;