TL;DR
When an edge of a polygon essentially overlaps with the edge of the intersection half-space, bad things happen. Among others, we are searching for an intersection point between two essentially overlapping lines. In code, this results in a determinant in a denominator being essentially zero. I think it would be safer not to remove polygon vertices which are very close to the edge of the halfspace. In code, this would mean replacing a line in ReachPolygon::intersect_halfspace with
bool ret = (a * vec_vertices[i].x + b * vec_vertices[i].y < c + 1e-10);
(note the added + 1e-10 part) or something similar.
The long story - My debugging journey
On a realistic scenario, I was occasionally receiving empty sets as results of the reachability analysis. I chased this down to ReachableSet::_compute_drivable_area_at_step occasionally computing flat sets when propagating a base set. All sets were then pruned, resulting in an empty reachability analysis result.
Wrong propagation
Thereby I noticed that the previous (non propagated) set was not fulfilling the assumption that it’s a convex polygon with vertices ordered in counter-clockwise direction (premise). A convex polygon should have exactly one extrema in any direction, so including the direction along the negative y-axis. As we can see below, this wasn’t the case, -0.025 and -0.325 are two different extremas (or at least whatever other extrema was following -0.325, sorry [1] is closed in image).
Wrong assumption
I wrote the following hacky function to test the premise
bool has_more_than_one_local_minimum_in_y_axis_direction(const shared_ptr<ReachPolygon> &polygon) {
auto polygon_vertices = polygon->vertices();
bool reached_minimum = false;
bool decreasing = false;
for (int i = 1; i < polygon_vertices.size() + 2; i++) {
auto &cur = polygon_vertices[i % polygon_vertices.size()];
auto &prev = polygon_vertices[(i-1) % polygon_vertices.size()];
// Increased, we have reached a minimum
if (decreasing && cur.y > prev.y) {
if (reached_minimum) {
// Multiple minimums
return true;
}
reached_minimum = true;
decreasing = false;
}
if (cur.y < prev.y) {
decreasing = true;
}
}
return false;
}
I would spare you the entire journey here, but I chased this down to reach::create_zero_state_polygon(double const& dt = 0.1, double const& a_min = -6, double const& a_max = 6), which will compute a non-convex polygon with the given parameters. More specifically, intersecting
[
(0.03, 0.6),
(0, 0.3), # r
(-0.03, -0.3), # OUTSIDE
(-0.03, -0.6), # q
(0, -0.3),
(0.03, 0.3),
]
with
a = -1.2
b = 0
c = 0.036
would result in
[
(-0.03, -0.6),
(0, -0.3),
(0.03, 0.3),
(0.03, 0.6),
(0, 0.3),
# NEW
(-0.03, -0.3),
(-0.03, -0.208), # problematic
]
This is because the edge between the following points is essentially on the edge of the half-space.
(-0.03, -0.3), # OUTSIDE
(-0.03, -0.6)
. I’ll give it a try again.
