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