Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Faster margin check with object database for AP_OABendyRuler.cpp 

Make faster implementation of object margin check for bendy ruler algorithm with reusing unchanging variables & unnecessary scale conversions
This commit is contained in:
Seunghwan Jo 2023-08-01 20:33:11 +09:00 committed by GitHub
parent 8499a6bd78
commit d503222ff4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 18 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -688,13 +688,28 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co
return false;
}
// convert cm to m without division
start_NEU *= 0.01f;
end_NEU *= 0.01f;
// faster implementation for Vector3f::closest_point_on_line_segment with reusing unchanged variables
// Point in the line segment defined by w1,w2 which is closest to point(p)
// 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;
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; i<oaDb->database_count(); i++) {
for (uint16_t i=0; i<oaDb->database_count(); ++i) {
const AP_OADatabase::OA_DbItem& item = oaDb->get_item(i);
const Vector3f point_cm = item.pos * 100.0f;
// 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;
// margin is distance between line segment and obstacle minus obstacle's radius
const float m = Vector3f::closest_distance_between_line_and_point(start_NEU, end_NEU, point_cm) * 0.01f - item.radius;
const float m = (point_vec - closest_point).length() - item.radius;
if (m < smallest_margin) {
smallest_margin = m;
}