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

AP_Math: SCurve check direction.length_squared is_zero

This commit is contained in:
Josh Henderson 2021-06-11 19:17:33 -04:00 committed by Andrew Tridgell
parent 309c32f91c
commit 32359e209d
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Math/SCurve.cpp
View File

@ -61,8 +61,9 @@ void SCurve::calculate_track(const Vector3f &origin, const Vector3f &destination
{ {
init(); init();
// leave track as zero length if origin and destination are the same // leave track as zero length if origin and destination are equal or if the new track length squared is zero
if (origin == destination) { const Vector3f track_temp = destination - origin;
if (track_temp.is_zero() || is_zero(track_temp.length_squared())) {
return; return;
} }
@ -84,7 +85,7 @@ void SCurve::calculate_track(const Vector3f &origin, const Vector3f &destination
return; return;
} }
track = destination - origin; track = track_temp;
const float track_length = track.length(); const float track_length = track.length();
if (is_zero(track_length)) { if (is_zero(track_length)) {
// avoid possible divide by zero // avoid possible divide by zero