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

AC_Avoid: move closest_point to AP_Math

This commit is contained in:
Peter Barker 2016-07-25 11:56:37 +10:00 committed by Randy Mackay
parent 2047d53470
commit 692ff22453
2 changed files with 1 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -122,7 +122,7 @@ void AC_Avoid::adjust_velocity_poly(const float kP, const float accel_cmss, Vect
Vector2f start = boundary[j];
Vector2f end = boundary[i];
// vector from current position to closest point on current edge
Vector2f limit_direction = closest_point(position_xy, start, end) - position_xy;
Vector2f limit_direction = Vector2f::closest_point(position_xy, start, end) - position_xy;
// distance to closest point
const float limit_distance = limit_direction.length();
if (!is_zero(limit_distance)) {
@ -201,31 +201,3 @@ float AC_Avoid::get_stopping_distance(const float kP, const float accel_cmss, co
return linear_distance + (speed*speed)/(2.0f*accel_cmss);
}
}
/*
* Returns the point closest to p on the line segment (v,w).
*
* Comments and implementation taken from
* http://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment
*/
Vector2f AC_Avoid::closest_point(Vector2f p, Vector2f v, Vector2f w) const
{
// length squared of line segment
const float l2 = (v - w).length_squared();
if (is_zero(l2)) {
// v == w case
return v;
}
// Consider the line extending the segment, parameterized as v + t (w - v).
// We find projection of point p onto the line.
// It falls where t = [(p-v) . (w-v)] / |w-v|^2
// We clamp t from [0,1] to handle points outside the segment vw.
const float t = ((p - v) * (w - v)) / l2;
if (t <= 0) {
return v;
} else if (t >= 1) {
return w;
} else {
return v + (w - v)*t;
}
}

5
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -75,11 +75,6 @@ private:
*/
float get_margin() const { return _fence.get_margin() * 100.0f; }
/*
* returns the point closest to p on the line segment (v,w)
*/
Vector2f closest_point(Vector2f p, Vector2f v, Vector2f w) const;
// external references
const AP_AHRS& _ahrs;
const AP_InertialNav& _inav;