From a7e4465a1df6b63b23542a2dc125a5dc957f7984 Mon Sep 17 00:00:00 2001 From: Seunghwan Jo Date: Wed, 2 Aug 2023 13:01:49 +0900 Subject: [PATCH] Fix & Update Obstacle Margin Check Fixed error in margin checking with object point vector Update obstacle check for better performance --- libraries/AC_Avoidance/AP_OABendyRuler.cpp | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index ee70b58221..5c5fc880c9 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp @@ -688,7 +688,7 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co return false; } - // convert cm to m without division + // // convert cm to m without division start_NEU *= 0.01f; end_NEU *= 0.01f; @@ -697,19 +697,25 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co // this is based on the explanation given here: www.fundza.com/vectors/point2line/index.html Vector3f line_vec = end_NEU - start_NEU; float line_length = line_vec.length(); - float scale = 1.0f / line_length; + bool length_ok = !is_zero(line_length); + // protection against divide by zero + float scale = length_ok ? 1.0f / line_length : 0.0f; const Vector3f line_unit_vec = line_vec * scale; + // check each obstacle's distance from segment float smallest_margin = FLT_MAX; for (uint16_t i=0; idatabase_count(); ++i) { const AP_OADatabase::OA_DbItem& item = oaDb->get_item(i); - // declare point_vec with protection against divide by zero - const Vector3f point_vec = ::is_zero(line_length) ? Vector3f(0.0f, 0.0f, 0.0f): item.pos - start_NEU; - const Vector3f scaled_point_vec = point_vec * scale; - const float dot_product = constrain_float(scaled_point_vec.dot(line_unit_vec), 0.0f, 1.0f); - const Vector3f closest_point = line_vec * dot_product; + const Vector3f point_vec = item.pos - start_NEU; + Vector3f closest_point = start_NEU; + // protection against divide by zero + if (length_ok) { + const Vector3f scaled_point_vec = point_vec * scale; + float dot_product = constrain_ftype(line_unit_vec * scaled_point_vec, 0.0f, 1.0f); + closest_point += line_vec * dot_product; + } // margin is distance between line segment and obstacle minus obstacle's radius - const float m = (point_vec - closest_point).length() - item.radius; + const float m = (closest_point - item.pos).length() - item.radius; if (m < smallest_margin) { smallest_margin = m; }