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

Fix: adding two vectors with nearly the same angle

Browse Source
This commit is contained in:
Pascal Serrarens 2023-03-24 15:42:56 +01:00
parent 295a2b3902
commit 99e4515358
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/Polar.cpp
View File

@ -34,8 +34,10 @@ Polar Polar::operator+(const Polar& v2) const {
float deltaAngle = Angle::Normalize(v2.angle - this->angle); float deltaAngle = Angle::Normalize(v2.angle - this->angle);
float rotation = deltaAngle < 0 ? 180 + deltaAngle : 180 - deltaAngle; float rotation = deltaAngle < 0 ? 180 + deltaAngle : 180 - deltaAngle;
if (rotation == 180 && v2.distance > 0) if (rotation == 180 && v2.distance > 0) {
return Polar::zero; // angle is too small, take this angle and add the distances
return Polar(this->angle, this->distance + v2.distance);
}
float newDistance = float newDistance =
Angle::CosineRuleSide(v2.distance, this->distance, rotation); Angle::CosineRuleSide(v2.distance, this->distance, rotation);