RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix nan issues in consine rule

Browse Source
This commit is contained in:
Pascal Serrarens 2023-01-27 11:45:02 +01:00
parent 4448ee150d
commit 755fc1961d
2 changed files with 23 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/Angle.cpp
View File

@ -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;
}

13
src/Polar.cpp
View File

@ -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);