From 32359e209dda3c6ce209c26c96d4a16fb47435a1 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Fri, 11 Jun 2021 19:17:33 -0400 Subject: [PATCH] AP_Math: SCurve check direction.length_squared is_zero --- libraries/AP_Math/SCurve.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Math/SCurve.cpp b/libraries/AP_Math/SCurve.cpp index 1b4e913e56..a3c000394c 100644 --- a/libraries/AP_Math/SCurve.cpp +++ b/libraries/AP_Math/SCurve.cpp @@ -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