mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-09 22:08:56 +08:00
Big refactor making line-polygon intersection robust
Rewrite everything in terms of primitive orient2d - https://perso.uclouvain.be/jean-francois.remacle/LMECA2170/robnotes.pdf Rather than trying to do it carefully in float, convert everything to fixed-point (int32 centimeters) so geometry questions can be answered exactly - http://www.science.smith.edu/~jorourke/books/compgeom.html The fixed point polygon vertices are stored in PlannerPolygons, which also exposes all necessary geometry operations. Three API layers: - Functions taking int32 coordinates directly - Functions taking vertex indices and calling the above - isLineFromPointToNodeIntersectingAnyInside, which takes the _float meter_ vehicle local position and gives the visible graph nodes Next steps: - Profile, performance should be better or similar - Geofence::checkIfLineViolatesAnyFence rebuilds a PlannerPolygons object every call, could optimise - Only the midpoint is check. A contrived nonconvex polygon with multiple vertex hits could slip through - Ignore or make test & fix - Related: midpoint could land outside due to integer division - Find if there is still a way to simplify / clarify the handling of the exclusion/inclusion difference, and write more tests for all the edge cases. - rename some things, for sure isLineFromPointToNodeIntersectingAnyInside, and comment where logic is not clear - lineSegmentIntersectsCircle is still float, could change to fixed - But do we even want to check circle intersection, or should we use the k-gon approximation consistently? - Input data validation: Complain if - Polygon non-simple (self intersecting) - Geofence features smaller than margin or distances smaller than 2*margin, so the shrinking gives topology changes - Next iteration could handle this even better by allowing some violations (of only the expanded polygons) but at high cost - Anything else
This commit is contained in:
Balduin
@@ -37,29 +37,259 @@
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
|
||||
// Layer-1 primitives are tested directly in int32 (no float, no cm semantics).
|
||||
|
||||
TEST(GeofenceUtilsTest, Orient2d)
|
||||
{
|
||||
// CCW turn -> +1, CW turn -> -1, collinear -> 0.
|
||||
EXPECT_EQ(1, geofence_utils::orient2d(0, 0, 1, 0, 0, 1));
|
||||
EXPECT_EQ(-1, geofence_utils::orient2d(0, 0, 1, 0, 0, -1));
|
||||
EXPECT_EQ(0, geofence_utils::orient2d(0, 0, 2, 2, 1, 1));
|
||||
EXPECT_EQ(0, geofence_utils::orient2d(0, 0, 2, 2, 3, 3));
|
||||
}
|
||||
|
||||
using SS = geofence_utils::SegSegResult;
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentsSharedEndpointNoIntersection)
|
||||
{
|
||||
// Two segments share endpoint (1,1) but go in different directions — no crossing
|
||||
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));
|
||||
// Collinear, second segment is a sub-interval of the first.
|
||||
EXPECT_EQ(SS::Collinear, geofence_utils::segmentsIntersect(0, 0, 2, 2, 1, 1, 2, 2));
|
||||
}
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentsCross)
|
||||
{
|
||||
// vertical line from origin straight up
|
||||
Vector2f p1(0.f, 0.f);
|
||||
Vector2f p2(0.f, 1.f);
|
||||
|
||||
Vector2f v1(-0.0001f, 0.0001f);
|
||||
Vector2f v2(1.f, 0.0001f);
|
||||
|
||||
EXPECT_TRUE(geofence_utils::segmentsIntersect(p1, p2, v1, v2));
|
||||
// Vertical ab vs horizontal cd just above the x-axis; strict interior crossing.
|
||||
EXPECT_EQ(SS::Cross, geofence_utils::segmentsIntersect(0, 0, 0, 100, -1, 1, 100, 1));
|
||||
}
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentsTouching)
|
||||
{
|
||||
// Endpoint c of cd lies strictly on the open ab.
|
||||
EXPECT_EQ(SS::CInsideAB, geofence_utils::segmentsIntersect(0, 0, 0, 200, 0, 100, 100, 100));
|
||||
// Argument order swapped: now endpoint a of the first segment is on cd.
|
||||
EXPECT_EQ(SS::AInsideCD, geofence_utils::segmentsIntersect(0, 100, 100, 100, 0, 0, 0, 200));
|
||||
|
||||
// Slanted variant.
|
||||
EXPECT_EQ(SS::CInsideAB, geofence_utils::segmentsIntersect(-100, 0, 100, 200, 0, 100, 100, 100));
|
||||
EXPECT_EQ(SS::AInsideCD, geofence_utils::segmentsIntersect(0, 100, 100, 100, -100, 0, 100, 200));
|
||||
}
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentsParallel)
|
||||
{
|
||||
// Disjoint, non-collinear.
|
||||
EXPECT_EQ(SS::Disjoint, geofence_utils::segmentsIntersect(0, 0, 300, 0, 1000, 1000, 4000, 2000));
|
||||
|
||||
// A segment with itself is collinear, not a proper crossing.
|
||||
EXPECT_EQ(SS::Collinear, geofence_utils::segmentsIntersect(0, 0, 300, 0, 0, 0, 300, 0));
|
||||
EXPECT_EQ(SS::Collinear,
|
||||
geofence_utils::segmentsIntersect(1000, 1000, 4000, 2000, 1000, 1000, 4000, 2000));
|
||||
}
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentsCollinearDisjoint)
|
||||
{
|
||||
// Two segments on the same line but with non-overlapping intervals.
|
||||
EXPECT_EQ(SS::Collinear, geofence_utils::segmentsIntersect(0, 0, 100, 0, 200, 0, 300, 0));
|
||||
}
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentPolygonExclusionOutside)
|
||||
{
|
||||
static constexpr int N = 4;
|
||||
|
||||
// Unit square (exclusion zone: inside disallowed)
|
||||
Vector2f p1(0.f, 0.f);
|
||||
Vector2f p2(1.f, 0.f);
|
||||
Vector2f p3(1.f, 1.f);
|
||||
Vector2f p4(0.f, 1.f);
|
||||
|
||||
Vector2f vertices[N] = {p1, p2, p3, p4};
|
||||
Vector2f vertices_rev[N] = {p4, p3, p2, p1};
|
||||
|
||||
// Line in a completely random place
|
||||
Vector2f l1(4.f, 5.f);
|
||||
Vector2f l2(5.f, 4.f);
|
||||
|
||||
// Outside an exclusion zone is allowed -> no intersection
|
||||
EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, false));
|
||||
EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, false));
|
||||
}
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentPolygonExclusionThroughEdge)
|
||||
{
|
||||
static constexpr int N = 4;
|
||||
|
||||
// Unit square (exclusion zone)
|
||||
Vector2f p1(0.f, 0.f);
|
||||
Vector2f p2(1.f, 0.f);
|
||||
Vector2f p3(1.f, 1.f);
|
||||
Vector2f p4(0.f, 1.f);
|
||||
|
||||
Vector2f vertices[N] = {p1, p2, p3, p4};
|
||||
Vector2f vertices_rev[N] = {p4, p3, p2, p1};
|
||||
|
||||
// Line obviously passing through an edge
|
||||
Vector2f l1(0.5f, 0.5f);
|
||||
Vector2f l2(0.5f, 1.5f);
|
||||
|
||||
// Crossing an edge always counts as intersection regardless of zone type
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, false));
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, false));
|
||||
|
||||
// Line through several edges
|
||||
Vector2f l3(0.5f, -0.5f);
|
||||
Vector2f l4(0.5f, 1.5f);
|
||||
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices, N, false));
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices_rev, N, false));
|
||||
}
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentPolygonExclusionInside)
|
||||
{
|
||||
static constexpr int N = 4;
|
||||
|
||||
// Unit square (exclusion zone)
|
||||
Vector2f p1(0.f, 0.f);
|
||||
Vector2f p2(1.f, 0.f);
|
||||
Vector2f p3(1.f, 1.f);
|
||||
Vector2f p4(0.f, 1.f);
|
||||
|
||||
Vector2f vertices[N] = {p1, p2, p3, p4};
|
||||
Vector2f vertices_rev[N] = {p4, p3, p2, p1};
|
||||
|
||||
// Line going exactly between opposite vertices
|
||||
Vector2f l1(0.f, 0.f);
|
||||
Vector2f l2(1.f, 1.f);
|
||||
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, false));
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, false));
|
||||
|
||||
// Line exactly touching sides
|
||||
Vector2f l3(0.5f, 0.0f);
|
||||
Vector2f l4(0.5f, 1.0f);
|
||||
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices, N, false));
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices_rev, N, false));
|
||||
|
||||
// Line completely in the interior of an exclusion zone -> disallowed
|
||||
Vector2f l5(0.2f, 0.2f);
|
||||
Vector2f l6(0.6f, 0.5f);
|
||||
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l5, l6, vertices, N, false));
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l5, l6, vertices_rev, N, false));
|
||||
}
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentPolygonExclusionTouching)
|
||||
{
|
||||
static constexpr int N = 4;
|
||||
|
||||
// Unit square (exclusion zone)
|
||||
Vector2f p1(0.f, 0.f);
|
||||
Vector2f p2(1.f, 0.f);
|
||||
Vector2f p3(1.f, 1.f);
|
||||
Vector2f p4(0.f, 1.f);
|
||||
|
||||
Vector2f vertices[N] = {p1, p2, p3, p4};
|
||||
Vector2f vertices_rev[N] = {p4, p3, p2, p1};
|
||||
|
||||
// Line exactly equal to one of the edges
|
||||
Vector2f l1(0.f, 0.f);
|
||||
Vector2f l2(1.f, 0.f);
|
||||
|
||||
EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, false));
|
||||
EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, false));
|
||||
|
||||
// Same line but longer at one end - fails
|
||||
EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, 2.f * l2, vertices, N, false));
|
||||
EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, 2.f * l2, vertices_rev, N, false));
|
||||
|
||||
// A line just skimming the (1, 1) vertex but staying outside
|
||||
Vector2f l3(2.f, 0.f);
|
||||
Vector2f l4(0.f, 2.f);
|
||||
|
||||
// Should be no intersection
|
||||
EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices, N, false));
|
||||
EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices_rev, N, false));
|
||||
|
||||
// But if we move the line a couple of cm towards the square
|
||||
l4(1) = 1.98f;
|
||||
|
||||
// We have a (strict) intersection
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices, N, false));
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices_rev, N, false));
|
||||
}
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentPolygonInclusionOutside)
|
||||
{
|
||||
static constexpr int N = 4;
|
||||
|
||||
// Unit square (inclusion zone: outside disallowed)
|
||||
Vector2f p1(0.f, 0.f);
|
||||
Vector2f p2(1.f, 0.f);
|
||||
Vector2f p3(1.f, 1.f);
|
||||
Vector2f p4(0.f, 1.f);
|
||||
|
||||
Vector2f vertices[N] = {p1, p2, p3, p4};
|
||||
Vector2f vertices_rev[N] = {p4, p3, p2, p1};
|
||||
|
||||
// Line in a completely random place outside the polygon
|
||||
Vector2f l1(4.f, 5.f);
|
||||
Vector2f l2(5.f, 4.f);
|
||||
|
||||
// Outside an inclusion zone is disallowed -> intersection
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, true));
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, true));
|
||||
}
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentPolygonInclusionThroughEdge)
|
||||
{
|
||||
static constexpr int N = 4;
|
||||
|
||||
// Unit square (inclusion zone)
|
||||
Vector2f p1(0.f, 0.f);
|
||||
Vector2f p2(1.f, 0.f);
|
||||
Vector2f p3(1.f, 1.f);
|
||||
Vector2f p4(0.f, 1.f);
|
||||
|
||||
Vector2f vertices[N] = {p1, p2, p3, p4};
|
||||
Vector2f vertices_rev[N] = {p4, p3, p2, p1};
|
||||
|
||||
// Line crossing an edge from inside to outside
|
||||
Vector2f l1(0.5f, 0.5f);
|
||||
Vector2f l2(0.5f, 1.5f);
|
||||
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, true));
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, true));
|
||||
|
||||
// Line through several edges
|
||||
Vector2f l3(0.5f, -0.5f);
|
||||
Vector2f l4(0.5f, 1.5f);
|
||||
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices, N, true));
|
||||
EXPECT_TRUE(geofence_utils::lineSegmentIntersectsPolygon(l3, l4, vertices_rev, N, true));
|
||||
}
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentPolygonInclusionInside)
|
||||
{
|
||||
static constexpr int N = 4;
|
||||
|
||||
// Unit square (inclusion zone)
|
||||
Vector2f p1(0.f, 0.f);
|
||||
Vector2f p2(1.f, 0.f);
|
||||
Vector2f p3(1.f, 1.f);
|
||||
Vector2f p4(0.f, 1.f);
|
||||
|
||||
Vector2f vertices[N] = {p1, p2, p3, p4};
|
||||
Vector2f vertices_rev[N] = {p4, p3, p2, p1};
|
||||
|
||||
// Line completely in the interior of an inclusion zone -> allowed
|
||||
Vector2f l1(0.2f, 0.2f);
|
||||
Vector2f l2(0.6f, 0.5f);
|
||||
|
||||
EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices, N, true));
|
||||
EXPECT_FALSE(geofence_utils::lineSegmentIntersectsPolygon(l1, l2, vertices_rev, N, true));
|
||||
}
|
||||
|
||||
|
||||
TEST(GeofenceUtilsTest, SegmentAndCircleIntersect)
|
||||
{
|
||||
// vertical line from origin straight up
|
||||
|
||||
@@ -37,17 +37,13 @@
|
||||
namespace geofence_utils
|
||||
{
|
||||
|
||||
bool insidePolygon(const matrix::Vector2f *vertices, int num_vertices,
|
||||
const matrix::Vector2f &point)
|
||||
namespace
|
||||
{
|
||||
bool c = false;
|
||||
|
||||
for (int i = 0, j = num_vertices - 1; i < num_vertices; j = i++) {
|
||||
insidePolygonUpdateState(c, vertices[i], vertices[j], point);
|
||||
}
|
||||
// Float-meter to int32-cm: the only conversion at the float/cm boundary.
|
||||
int32_t metersToCm(float m) { return static_cast<int32_t>(std::llroundf(m * 100.f)); }
|
||||
|
||||
return c;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
bool insideCircle(const matrix::Vector2<double> ¢er, float radius,
|
||||
const matrix::Vector2<double> &point)
|
||||
@@ -56,34 +52,152 @@ bool insideCircle(const matrix::Vector2<double> ¢er, float radius,
|
||||
return dist < radius;
|
||||
}
|
||||
|
||||
bool segmentsIntersect(const matrix::Vector2f &p1, const matrix::Vector2f &p2,
|
||||
const matrix::Vector2f &v1, const matrix::Vector2f &v2)
|
||||
int PlannerPolygons::addNode(const matrix::Vector2f &p)
|
||||
{
|
||||
// line intersection algorithm from wikipedia
|
||||
// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
|
||||
// Basic idea: a 2D vector formula for each line is created, consisting of a start point e.g. p1
|
||||
// and a direction vector pointing from p1 to p2. A running variable t [0,1] defines the position
|
||||
// on the line betwen p1 and p2. So in this case the formula for the line is P = p1 + t * (p2-p1).
|
||||
// The same is done for the second line and then both lines are set to be equal (to find the intersection).
|
||||
// We get two equations with two unknowns (t and u) which can be solved. If the denominator is zero, the lines are parallel
|
||||
// or coinciding, which means we can stop. If the solution for t and u is between 0 and 1, the line segments intersect, otherwise they don't.
|
||||
if (_num_nodes >= kMaxNodes) { return -1; }
|
||||
|
||||
float x1 = p1(0), y1 = p1(1);
|
||||
float x2 = p2(0), y2 = p2(1);
|
||||
float x3 = v1(0), y3 = v1(1);
|
||||
float x4 = v2(0), y4 = v2(1);
|
||||
setNode(_num_nodes, p);
|
||||
return _num_nodes++;
|
||||
}
|
||||
|
||||
float denominator = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
|
||||
void PlannerPolygons::setNode(int idx, const matrix::Vector2f &p)
|
||||
{
|
||||
_x_cm[idx] = metersToCm(p(0));
|
||||
_y_cm[idx] = metersToCm(p(1));
|
||||
}
|
||||
|
||||
if (fabsf(denominator) < FLT_EPSILON) {
|
||||
bool PlannerPolygons::addPolygon(const matrix::Vector2f *vertices_in, int num_vertices, bool is_inclusion_zone)
|
||||
{
|
||||
if (num_vertices < 3
|
||||
|| _num_polygons >= kMaxPolygons
|
||||
|| _num_nodes + num_vertices > kMaxNodes) {
|
||||
return false;
|
||||
}
|
||||
|
||||
float t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / denominator;
|
||||
float u = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / denominator;
|
||||
// Canonical orientation: walking vertices in stored order, INSIDE is on
|
||||
// the left of each edge. CCW for exclusion (bounded interior is forbidden),
|
||||
// CW for inclusion (unbounded exterior is forbidden). Reverse iff input
|
||||
// doesn't already match.
|
||||
const bool reverse = (isPolygonCCW(vertices_in, num_vertices) == is_inclusion_zone);
|
||||
|
||||
// lines intersect if both running variables are between 0 and 1
|
||||
return t > 0.0f && t < 1.0f && u > 0.0f && u < 1.0f;
|
||||
PolygonInfo &poly = _polygons[_num_polygons];
|
||||
poly.start_index = _num_nodes;
|
||||
poly.num_vertices = num_vertices;
|
||||
poly.inside_is_bounded = !is_inclusion_zone;
|
||||
|
||||
for (int i = 0; i < num_vertices; i++) {
|
||||
setNode(poly.start_index + i, vertices_in[reverse ? (num_vertices - 1 - i) : i]);
|
||||
}
|
||||
|
||||
_num_nodes += num_vertices;
|
||||
++_num_polygons;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Standard convex/reflex vertex test: at a convex vertex Inside is the small
|
||||
// wedge between the two incident edges (left of BOTH); at a reflex vertex it
|
||||
// is the large arc (left of EITHER).
|
||||
bool PlannerPolygons::pointInsideInteriorCone(const PolygonInfo &poly,
|
||||
int32_t px, int32_t py, int v) const
|
||||
{
|
||||
const int prev = poly.start_index + ((v == 0) ? poly.num_vertices - 1 : v - 1);
|
||||
const int curr = poly.start_index + v;
|
||||
const int next = poly.start_index + ((v + 1) % poly.num_vertices);
|
||||
|
||||
const int o_in = orient2d(_x_cm[prev], _y_cm[prev], _x_cm[curr], _y_cm[curr], px, py);
|
||||
const int o_out = orient2d(_x_cm[curr], _y_cm[curr], _x_cm[next], _y_cm[next], px, py);
|
||||
const int is_convex = orient2d(_x_cm[prev], _y_cm[prev], _x_cm[curr], _y_cm[curr], _x_cm[next], _y_cm[next]);
|
||||
|
||||
return is_convex > 0 // If convex == 0 (exact 180deg vertex) the below cases are equivalent
|
||||
? (o_in > 0 && o_out > 0) // Inside = intersection of both half-planes
|
||||
: (o_in > 0 || o_out > 0); // Inside = union of both half-planes
|
||||
}
|
||||
|
||||
bool PlannerPolygons::intersectsInsideOf(const PolygonInfo &poly,
|
||||
int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const
|
||||
{
|
||||
const int32_t mid_x = (s_x + e_x) / 2;
|
||||
const int32_t mid_y = (s_y + e_y) / 2;
|
||||
|
||||
// Single pass over polygon edges, doing two jobs at once:
|
||||
// (1) classify the test segment vs each polygon edge -- early-return on
|
||||
// a proper crossing, wedge-check polygon vertices that sit strictly
|
||||
// on the open test segment;
|
||||
// (2) accumulate Sunday's winding-number contribution for the midpoint,
|
||||
// used (with poly.inside_is_bounded) to classify the no-crossing
|
||||
// case as Inside or Outside.
|
||||
int wn = 0;
|
||||
bool mid_on_boundary = false;
|
||||
|
||||
for (int i = 0; i < poly.num_vertices; i++) {
|
||||
const int prev_idx = poly.start_index + ((i == 0) ? poly.num_vertices - 1 : i - 1);
|
||||
const int curr_idx = poly.start_index + i;
|
||||
const int32_t ax = _x_cm[prev_idx], ay = _y_cm[prev_idx];
|
||||
const int32_t bx = _x_cm[curr_idx], by = _y_cm[curr_idx];
|
||||
|
||||
// (1) test segment vs polygon edge (a, b)
|
||||
switch (segmentsIntersect(ax, ay, bx, by, s_x, s_y, e_x, e_y)) {
|
||||
case SegSegResult::Cross:
|
||||
return true;
|
||||
|
||||
case SegSegResult::BInsideCD:
|
||||
// polygon vertex `b` strictly on the open test segment -- a graze
|
||||
// unless the wedge classifies the two endpoints differently
|
||||
if (pointInsideInteriorCone(poly, s_x, s_y, i) !=
|
||||
pointInsideInteriorCone(poly, e_x, e_y, i)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// (2) midpoint winding contribution (Sunday)
|
||||
const int side = orient2d(ax, ay, bx, by, mid_x, mid_y);
|
||||
|
||||
if (side == 0 && collinearBetween(ax, ay, bx, by, mid_x, mid_y)) {
|
||||
mid_on_boundary = true;
|
||||
|
||||
} else if (ay <= mid_y) {
|
||||
if (by > mid_y && side > 0) { wn++; }
|
||||
|
||||
} else if (by <= mid_y && side < 0) { wn--; }
|
||||
}
|
||||
|
||||
// Midpoint on the boundary: segment runs along it; non-violating in either zone.
|
||||
if (mid_on_boundary) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// wn != 0 means the midpoint is in the bounded region. The only place
|
||||
// orientation polarity surfaces.
|
||||
const bool bounded = (wn != 0);
|
||||
return bounded == poly.inside_is_bounded;
|
||||
}
|
||||
|
||||
bool PlannerPolygons::intersectsAnyInside(int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const
|
||||
{
|
||||
for (int p = 0; p < _num_polygons; p++) {
|
||||
if (intersectsInsideOf(_polygons[p], s_x, s_y, e_x, e_y)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool PlannerPolygons::isLineBetweenNodesIntersectingAnyInside(int a, int b) const
|
||||
{
|
||||
if (a == b) { return false; }
|
||||
|
||||
return intersectsAnyInside(_x_cm[a], _y_cm[a], _x_cm[b], _y_cm[b]);
|
||||
}
|
||||
|
||||
bool PlannerPolygons::isLineFromPointToNodeIntersectingAnyInside(const matrix::Vector2f &p, int node_idx) const
|
||||
{
|
||||
return intersectsAnyInside(metersToCm(p(0)), metersToCm(p(1)), _x_cm[node_idx], _y_cm[node_idx]);
|
||||
}
|
||||
|
||||
bool lineSegmentIntersectsCircle(const matrix::Vector2f &start, const matrix::Vector2f &end,
|
||||
|
||||
@@ -38,6 +38,10 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstdlib>
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace geofence_utils
|
||||
@@ -84,30 +88,90 @@ bool insideCircle(const matrix::Vector2<double> ¢er, float radius,
|
||||
const matrix::Vector2<double> &point);
|
||||
|
||||
/**
|
||||
* Check if two line segments intersect (excluding endpoints).
|
||||
* Works in local Cartesian coordinates (meters).
|
||||
*
|
||||
* @param p1 first segment start in local frame
|
||||
* @param p2 first segment end in local frame
|
||||
* @param v1 second segment start in local frame
|
||||
* @param v2 second segment end in local frame
|
||||
* @return true if the segments intersect
|
||||
* Sign of the signed area of triangle abc -- the fundamental 2D orientation
|
||||
* predicate. +1 if c is left of a->b (CCW turn), -1 if right (CW turn),
|
||||
* 0 if collinear. Reference: O'Rourke, "Computational Geometry in C", 1.5.
|
||||
*/
|
||||
bool segmentsIntersect(const matrix::Vector2f &p1, const matrix::Vector2f &p2,
|
||||
const matrix::Vector2f &v1, const matrix::Vector2f &v2);
|
||||
inline int orient2d(int32_t ax, int32_t ay,
|
||||
int32_t bx, int32_t by,
|
||||
int32_t cx, int32_t cy)
|
||||
{
|
||||
const int64_t det = (static_cast<int64_t>(bx) - ax) * (static_cast<int64_t>(cy) - ay)
|
||||
- (static_cast<int64_t>(by) - ay) * (static_cast<int64_t>(cx) - ax);
|
||||
|
||||
if (det > 0) { return 1; }
|
||||
|
||||
if (det < 0) { return -1; }
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if a line segment intersects a circle.
|
||||
* Works in local Cartesian coordinates (meters).
|
||||
*
|
||||
* @param start segment start in local frame
|
||||
* @param end segment end in local frame
|
||||
* @param center circle center in local frame
|
||||
* @param radius circle radius in meters
|
||||
* @return true if the segment intersects the circle
|
||||
* For a, b, c known to be collinear (orient2d == 0), is c on the closed
|
||||
* segment ab?
|
||||
*/
|
||||
bool lineSegmentIntersectsCircle(const matrix::Vector2f &start, const matrix::Vector2f &end,
|
||||
const matrix::Vector2f ¢er, float radius);
|
||||
inline bool collinearBetween(int32_t ax, int32_t ay, int32_t bx, int32_t by, int32_t cx, int32_t cy)
|
||||
{
|
||||
if (std::abs(ax - bx) >= std::abs(ay - by)) {
|
||||
return (ax <= cx && cx <= bx) || (bx <= cx && cx <= ax);
|
||||
|
||||
} else {
|
||||
return (ay <= cy && cy <= by) || (by <= cy && cy <= ay);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Classification of the relative position of two segments ab and cd.
|
||||
*
|
||||
* Cross proper interior crossing (each segment strictly straddles
|
||||
* the other's supporting line)
|
||||
* AInsideCD a strictly between c and d (open interior of segment cd)
|
||||
* BInsideCD b strictly between c and d
|
||||
* CInsideAB c strictly between a and b
|
||||
* DInsideAB d strictly between a and b
|
||||
* Collinear ab and cd lie on the same supporting line (overlap or not)
|
||||
* Disjoint none of the above
|
||||
*
|
||||
* Reference: O'Rourke, "Computational Geometry in C" (2nd ed.), SegSegInt,
|
||||
* section 1.5.
|
||||
*/
|
||||
enum class SegSegResult {
|
||||
Disjoint,
|
||||
Cross,
|
||||
AInsideCD, BInsideCD,
|
||||
CInsideAB, DInsideAB,
|
||||
Collinear,
|
||||
};
|
||||
|
||||
inline SegSegResult segmentsIntersect(int32_t ax, int32_t ay, int32_t bx, int32_t by,
|
||||
int32_t cx, int32_t cy, int32_t dx, int32_t dy)
|
||||
{
|
||||
const int o1 = orient2d(ax, ay, bx, by, cx, cy);
|
||||
const int o2 = orient2d(ax, ay, bx, by, dx, dy);
|
||||
const int o3 = orient2d(cx, cy, dx, dy, ax, ay);
|
||||
const int o4 = orient2d(cx, cy, dx, dy, bx, by);
|
||||
|
||||
if (o1 && o2 && o3 && o4 && o1 != o2 && o3 != o4) { return SegSegResult::Cross; }
|
||||
|
||||
if (!o1 && !o2 && !o3 && !o4) { return SegSegResult::Collinear; }
|
||||
|
||||
// Endpoint strictly on the open interior of the other segment: orient2d
|
||||
// is zero (point on the supporting line), the point lies between the
|
||||
// other two via collinearBetween, and is not coincident with either.
|
||||
if (o3 == 0 && collinearBetween(cx, cy, dx, dy, ax, ay)
|
||||
&& !(ax == cx && ay == cy) && !(ax == dx && ay == dy)) { return SegSegResult::AInsideCD; }
|
||||
|
||||
if (o4 == 0 && collinearBetween(cx, cy, dx, dy, bx, by)
|
||||
&& !(bx == cx && by == cy) && !(bx == dx && by == dy)) { return SegSegResult::BInsideCD; }
|
||||
|
||||
if (o1 == 0 && collinearBetween(ax, ay, bx, by, cx, cy)
|
||||
&& !(cx == ax && cy == ay) && !(cx == bx && cy == by)) { return SegSegResult::CInsideAB; }
|
||||
|
||||
if (o2 == 0 && collinearBetween(ax, ay, bx, by, dx, dy)
|
||||
&& !(dx == ax && dy == ay) && !(dx == bx && dy == by)) { return SegSegResult::DInsideAB; }
|
||||
|
||||
return SegSegResult::Disjoint;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if a polygon's vertices are ordered counter-clockwise using the shoelace formula.
|
||||
@@ -128,17 +192,106 @@ bool isPolygonCCW(const matrix::Vector2f *vertices, int num_vertices);
|
||||
*
|
||||
* Returns false for degenerate polygons: fewer than 3 vertices, zero-length edges,
|
||||
* or antiparallel adjacent edges (polygon doubles back).
|
||||
*
|
||||
* @param vertices_in input vertices in local frame
|
||||
* @param num_vertices number of vertices
|
||||
* @param margin offset distance in meters
|
||||
* @param expand true to expand, false to shrink
|
||||
* @param vertices_out output array (must hold at least num_vertices elements)
|
||||
*/
|
||||
bool expandOrShrinkPolygon(const matrix::Vector2f *vertices_in, int num_vertices,
|
||||
float margin, bool expand,
|
||||
matrix::Vector2f *vertices_out);
|
||||
|
||||
/**
|
||||
* Read-only fence cache. Owns a flat int32-cm node buffer that holds every
|
||||
* position the planner cares about (polygon vertices, circle-approximation
|
||||
* vertices, destination, ...) and a polygon metadata table that slices into
|
||||
* it. Polygons are stored in canonical orientation: walking vertices in
|
||||
* stored order, the polygon's INSIDE region is always to the left of each
|
||||
* edge (CCW for exclusion zones, CW for inclusion zones).
|
||||
*
|
||||
* Three layers of intersection query:
|
||||
* (1) Free int32 predicates (orient2d, segmentsIntersect, collinearBetween)
|
||||
* at the top of this header -- raw scalar coordinates, no cm semantics.
|
||||
* (2) `isLineBetweenNodesIntersectingAnyInside(a, b)` -- node-to-node
|
||||
* visibility for the planner's V^2 hot path; returns false if a == b.
|
||||
* (3) `isLineFromPointToNodeIntersectingAnyInside(p, idx)` -- the only
|
||||
* float bridge, intended for the vehicle's current position.
|
||||
*/
|
||||
class PlannerPolygons
|
||||
{
|
||||
public:
|
||||
static constexpr int kMaxNodes = 100;
|
||||
static constexpr int kMaxPolygons = 16;
|
||||
|
||||
PlannerPolygons() = default;
|
||||
|
||||
void reset() { _num_nodes = 0; _num_polygons = 0; }
|
||||
|
||||
/// Append a single point to the node buffer. Returns its index, or -1 if full.
|
||||
int addNode(const matrix::Vector2f &p);
|
||||
|
||||
/// Replace the node at `idx` (used to update the destination slot in place).
|
||||
void setNode(int idx, const matrix::Vector2f &p);
|
||||
|
||||
/// Append a polygon: canonicalize orientation, quantize to cm, append
|
||||
/// vertices as nodes. Returns false on budget overflow or
|
||||
/// fewer-than-3-vertex input.
|
||||
bool addPolygon(const matrix::Vector2f *vertices_in, int num_vertices, bool is_inclusion_zone);
|
||||
|
||||
int numNodes() const { return _num_nodes; }
|
||||
|
||||
matrix::Vector2f node(int idx) const
|
||||
{
|
||||
return matrix::Vector2f{_x_cm[idx] / 100.f, _y_cm[idx] / 100.f};
|
||||
}
|
||||
|
||||
bool isLineBetweenNodesIntersectingAnyInside(int a, int b) const;
|
||||
bool isLineFromPointToNodeIntersectingAnyInside(const matrix::Vector2f &p, int node_idx) const;
|
||||
|
||||
private:
|
||||
struct PolygonInfo {
|
||||
int start_index;
|
||||
int num_vertices;
|
||||
bool inside_is_bounded;
|
||||
};
|
||||
|
||||
int32_t _x_cm[kMaxNodes];
|
||||
int32_t _y_cm[kMaxNodes];
|
||||
int _num_nodes{0};
|
||||
|
||||
PolygonInfo _polygons[kMaxPolygons];
|
||||
int _num_polygons{0};
|
||||
|
||||
bool intersectsAnyInside(int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const;
|
||||
bool intersectsInsideOf(const PolygonInfo &poly,
|
||||
int32_t s_x, int32_t s_y, int32_t e_x, int32_t e_y) const;
|
||||
bool pointInsideInteriorCone(const PolygonInfo &poly, int32_t px, int32_t py, int v) const;
|
||||
};
|
||||
|
||||
/// Convenience for unit tests and one-shot callers: build a transient
|
||||
/// PlannerPolygons with the given polygon and two extra nodes for the segment
|
||||
/// endpoints, then run the node-to-node query.
|
||||
inline bool lineSegmentIntersectsPolygon(const matrix::Vector2f &start, const matrix::Vector2f &end,
|
||||
const matrix::Vector2f *vertices, int num_vertices, bool is_inclusion_zone)
|
||||
{
|
||||
PlannerPolygons polys;
|
||||
|
||||
if (!polys.addPolygon(vertices, num_vertices, is_inclusion_zone)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return polys.isLineBetweenNodesIntersectingAnyInside(polys.addNode(start), polys.addNode(end));
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if a line segment intersects a circle.
|
||||
* Works in local Cartesian coordinates (meters).
|
||||
*
|
||||
* @param start segment start in local frame
|
||||
* @param end segment end in local frame
|
||||
* @param center circle center in local frame
|
||||
* @param radius circle radius in meters
|
||||
* @return true if the segment intersects the circle
|
||||
*/
|
||||
bool lineSegmentIntersectsCircle(const matrix::Vector2f &start, const matrix::Vector2f &end,
|
||||
const matrix::Vector2f ¢er, float radius);
|
||||
|
||||
/**
|
||||
* Map the upper triangular matrix WITHOUT the diagonal into a flat array. Caller must ensure i != j.
|
||||
*
|
||||
|
||||
@@ -49,17 +49,15 @@ public:
|
||||
{
|
||||
MapProjection ref{reference(0), reference(1)};
|
||||
|
||||
for (int i = 0; i < _num_vertices; i++) {
|
||||
matrix::Vector2f v1_local = ref.project(_vertices[i](0), _vertices[i](1));
|
||||
matrix::Vector2f v2_local = ref.project(_vertices[(i + 1) % _num_vertices](0),
|
||||
_vertices[(i + 1) % _num_vertices](1));
|
||||
matrix::Vector2f vertices_local[_num_vertices];
|
||||
|
||||
if (geofence_utils::segmentsIntersect(start_local, end_local, v1_local, v2_local)) {
|
||||
return true;
|
||||
}
|
||||
for (int i = 0; i < _num_vertices; i++) {
|
||||
vertices_local[i] = ref.project(_vertices[i](0), _vertices[i](1));
|
||||
}
|
||||
|
||||
return false;
|
||||
const bool is_inclusion_zone = (_fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
|
||||
return geofence_utils::lineSegmentIntersectsPolygon(start_local, end_local,
|
||||
vertices_local, _num_vertices, is_inclusion_zone);
|
||||
}
|
||||
|
||||
PolygonInfo getPolygonInfoByIndex(int index) override
|
||||
|
||||
@@ -107,7 +107,7 @@ bool GeofenceAvoidancePlanner::update_vertices(GeofenceInterface &geofence, floa
|
||||
return false;
|
||||
}
|
||||
|
||||
update_distances_between_vertices(geofence);
|
||||
update_distances_between_vertices();
|
||||
|
||||
// invalidate destination to force a refresh on the next update_destination()
|
||||
_destination_healthy = false;
|
||||
@@ -123,7 +123,8 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination(
|
||||
{
|
||||
const int num_polygons = geofence.getNumPolygons();
|
||||
|
||||
int node_index = 0;
|
||||
_polygons.reset();
|
||||
_num_circles = 0;
|
||||
|
||||
for (int poly_index = 0; poly_index < num_polygons; poly_index++) {
|
||||
|
||||
@@ -131,14 +132,24 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination(
|
||||
|
||||
if (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION || info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) {
|
||||
|
||||
const bool is_inclusion = (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
|
||||
|
||||
matrix::Vector2f local_in[info.vertex_count];
|
||||
matrix::Vector2f local_out[info.vertex_count];
|
||||
|
||||
for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) {
|
||||
local_in[vertex_idx] = get_vertex_local_position(poly_index, vertex_idx, geofence, _reference);
|
||||
}
|
||||
|
||||
const bool should_expand = (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION);
|
||||
// Cached polygon for line-violation queries: stores the ACTUAL
|
||||
// fence boundary (no margin). Graph-node positions below get
|
||||
// margin-offset separately so paths between them legitimately
|
||||
// run through the safety band without being flagged as violations.
|
||||
if (!_polygons.addPolygon(local_in, info.vertex_count, is_inclusion)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
matrix::Vector2f local_out[info.vertex_count];
|
||||
const bool should_expand = !is_inclusion;
|
||||
|
||||
if (!geofence_utils::expandOrShrinkPolygon(local_in, info.vertex_count, margin, should_expand,
|
||||
local_out)) {
|
||||
@@ -146,14 +157,21 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination(
|
||||
}
|
||||
|
||||
for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) {
|
||||
_positions[node_index] = local_out[vertex_idx];
|
||||
node_index++;
|
||||
if (_polygons.addNode(local_out[vertex_idx]) < 0) { return false; }
|
||||
}
|
||||
|
||||
} else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
|
||||
const matrix::Vector2f center = get_vertex_local_position(poly_index, 0, geofence, _reference);
|
||||
const float delta_angle = 2.f * M_PI_F / kCircleApproxVertices;
|
||||
if (_num_circles >= kMaxCircles) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const matrix::Vector2f center = get_vertex_local_position(poly_index, 0, geofence, _reference);
|
||||
_circles[_num_circles++] = {center, info.circle_radius};
|
||||
|
||||
// graph nodes for the circle: polygon approximation at the
|
||||
// margin-adjusted radius. The line-violation check uses the
|
||||
// actual circle (radius above), not this approximation.
|
||||
const float delta_angle = 2.f * M_PI_F / kCircleApproxVertices;
|
||||
float adjusted_radius;
|
||||
|
||||
if (info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
|
||||
@@ -169,36 +187,69 @@ bool GeofenceAvoidancePlanner::update_graph_nodes_without_start_and_destination(
|
||||
|
||||
for (int i = 0; i < kCircleApproxVertices; i++) {
|
||||
const float angle = i * delta_angle;
|
||||
_positions[node_index] = center + matrix::Vector2f{adjusted_radius * cosf(angle), adjusted_radius * sinf(angle)};
|
||||
node_index++;
|
||||
const matrix::Vector2f p = center + matrix::Vector2f{
|
||||
adjusted_radius * cosf(angle), adjusted_radius * sinf(angle)
|
||||
};
|
||||
|
||||
if (_polygons.addNode(p) < 0) { return false; }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// node_index equals the polygon vertex count; the destination is reserved one slot beyond
|
||||
_num_vertices = node_index;
|
||||
_num_nodes = node_index + 1; // +1 for the destination
|
||||
// destination occupies one extra slot beyond the polygon/circle vertices
|
||||
_num_vertices = _polygons.numNodes();
|
||||
_dest_idx = _polygons.addNode({0.f, 0.f});
|
||||
|
||||
if (_dest_idx < 0) { return false; }
|
||||
|
||||
_num_nodes = _polygons.numNodes();
|
||||
return true;
|
||||
}
|
||||
|
||||
void GeofenceAvoidancePlanner::update_distances_between_vertices(GeofenceInterface &geofence)
|
||||
bool GeofenceAvoidancePlanner::lineViolatesAnyCachedFenceBetweenNodes(int a, int b) const
|
||||
{
|
||||
if (_polygons.isLineBetweenNodesIntersectingAnyInside(a, b)) { return true; }
|
||||
|
||||
const matrix::Vector2f start = _polygons.node(a);
|
||||
const matrix::Vector2f end = _polygons.node(b);
|
||||
|
||||
for (int i = 0; i < _num_circles; i++) {
|
||||
if (geofence_utils::lineSegmentIntersectsCircle(start, end, _circles[i].center, _circles[i].radius)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool GeofenceAvoidancePlanner::lineViolatesAnyCachedFenceFromPoint(const matrix::Vector2f &p, int node_idx) const
|
||||
{
|
||||
if (_polygons.isLineFromPointToNodeIntersectingAnyInside(p, node_idx)) { return true; }
|
||||
|
||||
const matrix::Vector2f end = _polygons.node(node_idx);
|
||||
|
||||
for (int i = 0; i < _num_circles; i++) {
|
||||
if (geofence_utils::lineSegmentIntersectsCircle(p, end, _circles[i].center, _circles[i].radius)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void GeofenceAvoidancePlanner::update_distances_between_vertices()
|
||||
{
|
||||
perf_begin(_setup_distances_perf);
|
||||
|
||||
// loop through all possible vertex to vertex combinations and store the distance between them
|
||||
for (int i = 0; i < _num_vertices; i++) {
|
||||
for (int j = i + 1; j < _num_vertices; j++) {
|
||||
const size_t idx = geofence_utils::symmetricPairIndex(i, j, _num_nodes);
|
||||
|
||||
const bool clear = !geofence.checkIfLineViolatesAnyFence(_positions[i],
|
||||
_positions[j], _reference);
|
||||
|
||||
if (clear) {
|
||||
_distances[idx] = (_positions[i] - _positions[j]).norm();
|
||||
if (lineViolatesAnyCachedFenceBetweenNodes(i, j)) {
|
||||
_distances[idx] = INFINITY;
|
||||
|
||||
} else {
|
||||
_distances[idx] = INFINITY;
|
||||
_distances[idx] = (_polygons.node(i) - _polygons.node(j)).norm();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -217,28 +268,21 @@ void GeofenceAvoidancePlanner::update_destination(const matrix::Vector2d &destin
|
||||
matrix::Vector2f dest_local;
|
||||
ref.project(destination(0), destination(1), dest_local(0), dest_local(1));
|
||||
|
||||
const int dest_idx = _num_nodes - 1;
|
||||
|
||||
if (_destination_healthy && (dest_local - _positions[dest_idx]).norm() < FLT_EPSILON) {
|
||||
// no change in destination, skip the update
|
||||
if (_destination_healthy && (dest_local - _polygons.node(_dest_idx)).norm() < FLT_EPSILON) {
|
||||
return;
|
||||
}
|
||||
|
||||
perf_begin(_update_destination_perf);
|
||||
_positions[dest_idx] = dest_local;
|
||||
_polygons.setNode(_dest_idx, dest_local);
|
||||
|
||||
// distances from each vertex to destination
|
||||
for (int graph_idx = 0; graph_idx < _num_vertices; graph_idx++) {
|
||||
const size_t dist_idx = geofence_utils::symmetricPairIndex(graph_idx, dest_idx, _num_nodes);
|
||||
const size_t dist_idx = geofence_utils::symmetricPairIndex(graph_idx, _dest_idx, _num_nodes);
|
||||
|
||||
const bool clear = !geofence.checkIfLineViolatesAnyFence(_positions[graph_idx],
|
||||
_positions[dest_idx], _reference);
|
||||
|
||||
if (clear) {
|
||||
_distances[dist_idx] = (_positions[graph_idx] - _positions[dest_idx]).norm();
|
||||
if (lineViolatesAnyCachedFenceBetweenNodes(graph_idx, _dest_idx)) {
|
||||
_distances[dist_idx] = INFINITY;
|
||||
|
||||
} else {
|
||||
_distances[dist_idx] = INFINITY;
|
||||
_distances[dist_idx] = (_polygons.node(graph_idx) - _polygons.node(_dest_idx)).norm();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -275,9 +319,9 @@ int GeofenceAvoidancePlanner::set_start_and_plan_path_to_destination(matrix::Vec
|
||||
|
||||
for (int node_idx = 0; node_idx < _num_nodes; node_idx++) {
|
||||
// check if node is reachable from this position
|
||||
if (!geofence.checkIfLineViolatesAnyFence(_start_local, _positions[node_idx], _reference)) {
|
||||
if (!lineViolatesAnyCachedFenceFromPoint(_start_local, node_idx)) {
|
||||
|
||||
if (node_idx == _num_nodes - 1) {
|
||||
if (node_idx == _dest_idx) {
|
||||
// destination is directly reachable from start: no detour planning needed.
|
||||
// If the vehicle is already at the start, no waypoints at all are needed.
|
||||
// Otherwise we still need the anchor (point 0) so the vehicle flies back into the fence first.
|
||||
@@ -286,7 +330,7 @@ int GeofenceAvoidancePlanner::set_start_and_plan_path_to_destination(matrix::Vec
|
||||
return start_is_current_position ? 0 : 1;
|
||||
}
|
||||
|
||||
const float cost = (_start_local - _positions[node_idx]).norm() + _best_distance[node_idx];
|
||||
const float cost = (_start_local - _polygons.node(node_idx)).norm() + _best_distance[node_idx];
|
||||
|
||||
if (cost < best_cost) {
|
||||
best_cost = cost;
|
||||
@@ -307,7 +351,7 @@ int GeofenceAvoidancePlanner::set_start_and_plan_path_to_destination(matrix::Vec
|
||||
int next = _next_node_buffer[current_index];
|
||||
_num_path_points++;
|
||||
|
||||
if (next == _num_nodes - 1 || next < 0) {
|
||||
if (next == _dest_idx || next < 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -356,6 +400,7 @@ matrix::Vector2d GeofenceAvoidancePlanner::get_point_at_index(int index) const
|
||||
|
||||
matrix::Vector2d global;
|
||||
MapProjection ref{_reference(0), _reference(1)};
|
||||
ref.reproject(_positions[next](0), _positions[next](1), global(0), global(1));
|
||||
const matrix::Vector2f p = _polygons.node(next);
|
||||
ref.reproject(p(0), p(1), global(0), global(1));
|
||||
return global;
|
||||
}
|
||||
|
||||
@@ -42,9 +42,11 @@
|
||||
#include <cstdint>
|
||||
#include <matrix/math.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/geofence/geofence_utils.h>
|
||||
#include "geofence_interface.h"
|
||||
|
||||
static constexpr int kMaxNodes = 100;
|
||||
static constexpr int kMaxCircles = 16;
|
||||
static constexpr int kCircleApproxVertices = 8;
|
||||
|
||||
struct PlannedPath {
|
||||
@@ -148,11 +150,26 @@ private:
|
||||
|
||||
matrix::Vector2f _start_local;
|
||||
|
||||
matrix::Vector2f _positions[kMaxNodes];
|
||||
int _num_nodes{0};
|
||||
int _num_vertices{0};
|
||||
int _dest_idx{-1};
|
||||
matrix::Vector2<double> _reference; // lat/lon anchor of the local frame
|
||||
|
||||
// Cached, read-only fence representation. _polygons owns the master
|
||||
// node buffer (polygon vertices + circle approximation vertices +
|
||||
// destination); _circles holds the analytical circle metadata for the
|
||||
// line-vs-circle violation check. Built once per `update_vertices`
|
||||
// (= geofence geometry change); reused by every line-violates-fence
|
||||
// query during distance setup, destination updates, and start-anchor
|
||||
// selection.
|
||||
struct CachedCircle {
|
||||
matrix::Vector2f center;
|
||||
float radius;
|
||||
};
|
||||
geofence_utils::PlannerPolygons _polygons;
|
||||
CachedCircle _circles[kMaxCircles];
|
||||
int _num_circles{0};
|
||||
|
||||
bool _polygons_healthy{false};
|
||||
bool _destination_healthy{false};
|
||||
bool _start_is_current_position{true};
|
||||
@@ -163,9 +180,14 @@ private:
|
||||
perf_counter_t _plan_path_perf{perf_alloc(PC_ELAPSED, "rtl_planner: plan path")};
|
||||
|
||||
bool update_graph_nodes_without_start_and_destination(GeofenceInterface &geofence, float margin);
|
||||
void update_distances_between_vertices(GeofenceInterface &geofence);
|
||||
void update_distances_between_vertices();
|
||||
void planPath();
|
||||
|
||||
// Read-only query against the cached fence: union of polygon (index-based)
|
||||
// and circle (analytic, float) checks. No geofence dataman roundtrips.
|
||||
bool lineViolatesAnyCachedFenceBetweenNodes(int a, int b) const;
|
||||
bool lineViolatesAnyCachedFenceFromPoint(const matrix::Vector2f &p, int node_idx) const;
|
||||
|
||||
bool lat_lon_within_bounds(const matrix::Vector2<double> &lat_lon);
|
||||
|
||||
};
|
||||
|
||||
@@ -86,9 +86,7 @@ Geofence::Geofence(Navigator *navigator) :
|
||||
|
||||
Geofence::~Geofence()
|
||||
{
|
||||
if (_polygons) {
|
||||
delete[](_polygons);
|
||||
}
|
||||
delete[] _polygons;
|
||||
}
|
||||
|
||||
void Geofence::run()
|
||||
@@ -252,7 +250,7 @@ void Geofence::_updateFence()
|
||||
memcpy(new_polygons, _polygons, sizeof(PolygonInfo) * _num_polygons);
|
||||
}
|
||||
|
||||
delete[](_polygons);
|
||||
delete[] _polygons;
|
||||
_polygons = new_polygons;
|
||||
|
||||
} else {
|
||||
@@ -275,7 +273,7 @@ void Geofence::_updateFence()
|
||||
|
||||
} else {
|
||||
polygon.vertex_count = mission_fence_point.vertex_count;
|
||||
current_seq += mission_fence_point.vertex_count;
|
||||
current_seq += polygon.vertex_count;
|
||||
}
|
||||
|
||||
// check if requiremetns for Home location are met
|
||||
@@ -738,35 +736,32 @@ bool Geofence::checkIfLineViolatesAnyFence(const matrix::Vector2f &start_local,
|
||||
|
||||
if (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION || info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) {
|
||||
|
||||
matrix::Vector2f vertices_local[info.vertex_count];
|
||||
dm_item_t fence_dataman_id{static_cast<dm_item_t>(_stats.dataman_id)};
|
||||
bool load_success = true;
|
||||
|
||||
for (int vertex_idx = 0; vertex_idx < info.vertex_count; vertex_idx++) {
|
||||
mission_fence_point_s vertex_current{};
|
||||
mission_fence_point_s vertex_previous{};
|
||||
mission_fence_point_s vertex{};
|
||||
|
||||
int prev_idx = vertex_idx == 0 ? info.vertex_count - 1 : vertex_idx - 1;
|
||||
|
||||
dm_item_t fence_dataman_id{static_cast<dm_item_t>(_stats.dataman_id)};
|
||||
bool success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + vertex_idx,
|
||||
reinterpret_cast<uint8_t *>(&vertex_current),
|
||||
sizeof(mission_fence_point_s));
|
||||
|
||||
if (!success) {
|
||||
if (!_dataman_cache.loadWait(fence_dataman_id, info.dataman_index + vertex_idx,
|
||||
reinterpret_cast<uint8_t *>(&vertex),
|
||||
sizeof(mission_fence_point_s))) {
|
||||
load_success = false;
|
||||
break;
|
||||
}
|
||||
|
||||
success = _dataman_cache.loadWait(fence_dataman_id, info.dataman_index + prev_idx,
|
||||
reinterpret_cast<uint8_t *>(&vertex_previous),
|
||||
sizeof(mission_fence_point_s));
|
||||
vertices_local[vertex_idx] = ref.project(vertex.lat, vertex.lon);
|
||||
}
|
||||
|
||||
if (!success) {
|
||||
break;
|
||||
}
|
||||
if (!load_success) {
|
||||
continue;
|
||||
}
|
||||
|
||||
matrix::Vector2f vertex_current_local = ref.project(vertex_current.lat, vertex_current.lon);
|
||||
matrix::Vector2f vertex_previous_local = ref.project(vertex_previous.lat, vertex_previous.lon);
|
||||
const bool is_inclusion_zone = (info.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION);
|
||||
|
||||
if (geofence_utils::segmentsIntersect(start_local, end_local, vertex_current_local, vertex_previous_local)) {
|
||||
return true;
|
||||
}
|
||||
if (geofence_utils::lineSegmentIntersectsPolygon(start_local, end_local,
|
||||
vertices_local, info.vertex_count, is_inclusion_zone)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
} else if (info.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION || info.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
|
||||
|
||||
Reference in New Issue
Block a user