Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: Dijkstra: don't consider points in corners

Browse Source
This commit is contained in:
Iampete1 2021-10-11 03:22:34 +01:00 committed by Randy Mackay
parent ae9300134a
commit 8937802f77
1 changed files with 48 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -289,14 +289,9 @@ bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OAD
uint16_t num_points;
const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
// expand array if required
if (!_inclusion_polygon_pts.expand_to_hold(_inclusion_polygon_numpoints + num_points)) {
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
return false;
}
// for each point in inclusion polygon
// Note: boundary is "unclosed" meaning the last point is *not* the same as the first
uint16_t new_points = 0;
for (uint16_t j = 0; j < num_points; j++) {
// find points before and after current point (relative to current point)
@ -316,26 +311,40 @@ bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OAD
after_pt.normalize();
// calculate intermediate point and scale to margin
Vector2f intermediate_pt = (after_pt + before_pt) * 0.5f;
float intermediate_len = intermediate_pt.length();
intermediate_pt *= (margin_cm / intermediate_len);
Vector2f intermediate_pt = after_pt + before_pt;
intermediate_pt.normalize();
intermediate_pt *= margin_cm;
// find final point which is outside the inside polygon
uint16_t next_index = _inclusion_polygon_numpoints + j;
_inclusion_polygon_pts[next_index] = boundary[j] + intermediate_pt;
if (Polygon_outside(_inclusion_polygon_pts[next_index], boundary, num_points)) {
_inclusion_polygon_pts[next_index] = boundary[j] - intermediate_pt;
if (Polygon_outside(_inclusion_polygon_pts[next_index], boundary, num_points)) {
Vector2f temp_point = boundary[j] + intermediate_pt;
if (Polygon_outside(temp_point, boundary, num_points)) {
intermediate_pt *= -1.0;
temp_point = boundary[j] + intermediate_pt;
if (Polygon_outside(temp_point, boundary, num_points)) {
// could not find a point on either side that was outside the exclusion polygon so fail
// this may happen if the exclusion polygon has overlapping lines
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_LINES;
return false;
}
}
// don't add points in corners
if (fabsf(intermediate_pt.angle() - before_pt.angle()) < M_PI_2) {
continue;
}
// expand array if required
if (!_inclusion_polygon_pts.expand_to_hold(_inclusion_polygon_numpoints + new_points + 1)) {
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
return false;
}
// add point
_inclusion_polygon_pts[_inclusion_polygon_numpoints + new_points] = temp_point;
new_points++;
}
// update total number of points
_inclusion_polygon_numpoints += num_points;
_inclusion_polygon_numpoints += new_points;
}
return true;
}
@ -381,14 +390,9 @@ bool AP_OADijkstra::create_exclusion_polygon_with_margin(float margin_cm, AP_OAD
uint16_t num_points;
const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
// expand array if required
if (!_exclusion_polygon_pts.expand_to_hold(_exclusion_polygon_numpoints + num_points)) {
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
return false;
}
// for each point in exclusion polygon
// Note: boundary is "unclosed" meaning the last point is *not* the same as the first
uint16_t new_points = 0;
for (uint16_t j = 0; j < num_points; j++) {
// find points before and after current point (relative to current point)
@ -408,26 +412,40 @@ bool AP_OADijkstra::create_exclusion_polygon_with_margin(float margin_cm, AP_OAD
after_pt.normalize();
// calculate intermediate point and scale to margin
Vector2f intermediate_pt = (after_pt + before_pt) * 0.5f;
float intermediate_len = intermediate_pt.length();
intermediate_pt *= (margin_cm / intermediate_len);
Vector2f intermediate_pt = after_pt + before_pt;
intermediate_pt.normalize();
intermediate_pt *= margin_cm;
// find final point which is outside the original polygon
uint16_t next_index = _exclusion_polygon_numpoints + j;
_exclusion_polygon_pts[next_index] = boundary[j] + intermediate_pt;
if (!Polygon_outside(_exclusion_polygon_pts[next_index], boundary, num_points)) {
_exclusion_polygon_pts[next_index] = boundary[j] - intermediate_pt;
if (!Polygon_outside(_exclusion_polygon_pts[next_index], boundary, num_points)) {
Vector2f temp_point = boundary[j] + intermediate_pt;
if (!Polygon_outside(temp_point, boundary, num_points)) {
intermediate_pt *= -1;
temp_point = boundary[j] + intermediate_pt;
if (!Polygon_outside(temp_point, boundary, num_points)) {
// could not find a point on either side that was outside the exclusion polygon so fail
// this may happen if the exclusion polygon has overlapping lines
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_LINES;
return false;
}
}
// don't add points in corners
if (fabsf(intermediate_pt.angle() - before_pt.angle()) < M_PI_2) {
continue;
}
// expand array if required
if (!_exclusion_polygon_pts.expand_to_hold(_exclusion_polygon_numpoints + new_points + 1)) {
err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
return false;
}
// add point
_exclusion_polygon_pts[_exclusion_polygon_numpoints + new_points] = temp_point;
new_points++;
}
// update total number of points
_exclusion_polygon_numpoints += num_points;
_exclusion_polygon_numpoints += new_points;
}
return true;
}
@ -655,11 +673,6 @@ bool AP_OADijkstra::create_fence_visgraph(AP_OADijkstra_Error &err_id)
// returns true on success
bool AP_OADijkstra::update_visgraph(AP_OAVisGraph& visgraph, const AP_OAVisGraph::OAItemID& oaid, const Vector2f &position, bool add_extra_position, Vector2f extra_position)
{
// exit immediately if no fence (with margin) points
if (total_numpoints() == 0) {
return false;
}
// clear visibility graph
visgraph.clear();