diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index df374f34b6..ac8956cd16 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/libraries/AC_Avoidance/AP_OADijkstra.cpp @@ -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();