From 4ec8602de21abee90d9031bac1e9913d1f8aa035 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Sat, 7 Mar 2020 23:48:32 +0530 Subject: [PATCH] AC_Avoid: Remove unnecessary sanity checks --- libraries/AC_Avoidance/AC_Avoid.cpp | 10 ++-------- libraries/AC_Avoidance/AP_OABendyRuler.cpp | 12 ++---------- libraries/AC_Avoidance/AP_OADijkstra.cpp | 14 +++----------- 3 files changed, 7 insertions(+), 29 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index bca0340cce..e585356ef8 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -406,10 +406,7 @@ void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float for (uint8_t i = 0; i < num_inclusion_polygons; i++) { uint16_t num_points; const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points); - if (num_points < 3) { - // ignore exclusion polygons with less than 3 points - continue; - } + // adjust velocity adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, fence->get_margin(), dt, true); } @@ -419,10 +416,7 @@ void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float for (uint8_t i = 0; i < num_exclusion_polygons; i++) { uint16_t num_points; const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points); - if (num_points < 3) { - // ignore exclusion polygons with less than 3 points - continue; - } + // adjust velocity adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, fence->get_margin(), dt, false); } diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index d39daed243..b143eab928 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp @@ -236,11 +236,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Lo for (uint8_t i = 0; i < num_inclusion_polygons; i++) { uint16_t num_points; const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points); - if (num_points < 3) { - // ignore exclusion polygons with less than 3 points - continue; - } - + // if outside the fence margin is the closest distance but with negative sign const float sign = Polygon_outside(start_NE, boundary, num_points) ? -1.0f : 1.0f; @@ -256,11 +252,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Lo for (uint8_t i = 0; i < num_exclusion_polygons; i++) { uint16_t num_points; const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points); - if (num_points < 3) { - // ignore exclusion polygons with less than 3 points - continue; - } - + // if start is inside the polygon the margin's sign is reversed const float sign = Polygon_outside(start_NE, boundary, num_points) ? 1.0f : -1.0f; diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index 628aed7a00..7c29cd7030 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/libraries/AC_Avoidance/AP_OADijkstra.cpp @@ -265,10 +265,6 @@ bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OAD for (uint8_t i = 0; i < num_inclusion_polygons; i++) { uint16_t num_points; const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points); - if (num_points < 3) { - // ignore exclusion polygons with less than 3 points - continue; - } // expand array if required if (!_inclusion_polygon_pts.expand_to_hold(_inclusion_polygon_numpoints + num_points)) { @@ -356,11 +352,7 @@ bool AP_OADijkstra::create_exclusion_polygon_with_margin(float margin_cm, AP_OAD for (uint8_t i = 0; i < num_exclusion_polygons; i++) { uint16_t num_points; const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points); - if (num_points < 3) { - // ignore exclusion polygons with less than 3 points - continue; - } - + // 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; @@ -533,7 +525,7 @@ bool AP_OADijkstra::intersects_fence(const Vector2f &seg_start, const Vector2f & uint16_t num_points = 0; for (uint8_t i = 0; i < fence->polyfence().get_inclusion_polygon_count(); i++) { const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points); - if ((boundary != nullptr) && (num_points >= 3)) { + if (boundary != nullptr) { Vector2f intersection; if (Polygon_intersects(boundary, num_points, seg_start, seg_end, intersection)) { return true; @@ -544,7 +536,7 @@ bool AP_OADijkstra::intersects_fence(const Vector2f &seg_start, const Vector2f & // determine if segment crosses any of the exclusion polygons for (uint8_t i = 0; i < fence->polyfence().get_exclusion_polygon_count(); i++) { const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points); - if ((boundary != nullptr) && (num_points >= 3)) { + if (boundary != nullptr) { Vector2f intersection; if (Polygon_intersects(boundary, num_points, seg_start, seg_end, intersection)) { return true;