From 755fc1961deb569c704a019cc5655132cbfe3412 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Fri, 27 Jan 2023 11:45:02 +0100
Subject: [PATCH] Fix nan issues in consine rule

---
 src/Angle.cpp | 20 +++++++++++++++++---
 src/Polar.cpp | 13 ++++++-------
 2 files changed, 23 insertions(+), 10 deletions(-)

diff --git a/src/Angle.cpp b/src/Angle.cpp
index db0097d..54df26e 100644
--- a/src/Angle.cpp
+++ b/src/Angle.cpp
@@ -2,8 +2,10 @@
 // License, v. 2.0.If a copy of the MPL was not distributed with this
 // file, You can obtain one at https ://mozilla.org/MPL/2.0/.
 
-#include "Angle.h"
+#include <Arduino.h>
+
 #include <math.h>
+#include "Angle.h"
 #include "FloatSingle.h"
 
 const float Angle::Rad2Deg = 57.29578F;
@@ -41,7 +43,12 @@ float Angle::MoveTowards(float fromAngle, float toAngle, float maxAngle) {
 float Angle::CosineRuleSide(float a, float b, float gamma) {
   float a2 = a * a;
   float b2 = b * b;
-  float c = sqrtf(a2 + b2 - 2 * a * b * cos(gamma * Angle::Deg2Rad));
+  float d = a2 + b2 - 2 * a * b * cos(gamma * Angle::Deg2Rad);
+  // Catch edge cases where float inacuracies lead tot nans
+  if (d < 0)
+    return 0;
+
+  float c = sqrtf(d);
   return c;
 }
 
@@ -49,7 +56,14 @@ float Angle::CosineRuleAngle(float a, float b, float c) {
   float a2 = a * a;
   float b2 = b * b;
   float c2 = c * c;
-  float gamma = acos((a2 + b2 - c2) / (2 * a * b)) * Angle::Rad2Deg;
+  float d = (a2 + b2 - c2) / (2 * a * b);
+  // Catch edge cases where float inacuracies lead tot nans
+  if (d >= 1)
+    return 0;
+  if (d <= -1)
+    return 180;
+
+  float gamma = acos(d) * Angle::Rad2Deg;
   return gamma;
 }
 
diff --git a/src/Polar.cpp b/src/Polar.cpp
index 9334733..d9d15c0 100644
--- a/src/Polar.cpp
+++ b/src/Polar.cpp
@@ -28,22 +28,21 @@ float Polar::Distance(const Polar& v1, const Polar& v2) {
 }
 
 Polar Polar::operator+(const Polar& v2) const {
+  if (v2.distance == 0)
+    return Polar(this->angle, this->distance);
+
   float deltaAngle = Angle::Normalize(v2.angle - this->angle);
   float rotation = deltaAngle < 0 ? 180 + deltaAngle : 180 - deltaAngle;
 
   if (rotation == 180 && v2.distance > 0)
     return Polar::zero;
 
-  // return Polar(this->angle + v2.angle, this->distance + v2.distance);
   float newDistance =
       Angle::CosineRuleSide(v2.distance, this->distance, rotation);
 
-  float angle = rotation;
-  if (newDistance > 0.001F &&
-      this->distance > 0.001F)  // nonzero because float inaccuracies may still
-                                // lead to problems with small number
-                                // plus, it is more efficient....
-    angle = Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
+  float angle =
+      Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
+
   float newAngle = deltaAngle < 0 ? Angle::Normalize(this->angle - angle)
                                   : Angle::Normalize(this->angle + angle);
   Polar vector = Polar(newAngle, newDistance);