77 lines
1.9 KiB
C++
77 lines
1.9 KiB
C++
#include <math.h>
|
|
|
|
#include "Angle.h"
|
|
#include "Polar.h"
|
|
|
|
Polar::Polar() {
|
|
angle = 0.0F;
|
|
distance = 0.0F;
|
|
}
|
|
|
|
Polar::Polar(float newAngle, float newDistance) {
|
|
// distance should always be 0 or greater
|
|
if (newDistance < 0) {
|
|
angle = Angle::Normalize(newAngle - 180);
|
|
distance = -newDistance;
|
|
} else {
|
|
angle = Angle::Normalize(newAngle);
|
|
distance = newDistance;
|
|
}
|
|
}
|
|
|
|
const Polar Polar::zero = Polar(0, 0);
|
|
|
|
float Polar::Distance(const Polar &v1, const Polar &v2) {
|
|
float d =
|
|
Angle::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle);
|
|
return d;
|
|
}
|
|
|
|
Polar Polar::operator+(const Polar &v2) const {
|
|
if (v2.distance == 0)
|
|
return Polar(this->angle, this->distance);
|
|
if (this->distance == 0)
|
|
return v2;
|
|
|
|
float deltaAngle = Angle::Normalize(v2.angle - this->angle);
|
|
float rotation = deltaAngle < 0 ? 180 + deltaAngle : 180 - deltaAngle;
|
|
|
|
if (rotation == 180 && v2.distance > 0) {
|
|
// angle is too small, take this angle and add the distances
|
|
return Polar(this->angle, this->distance + v2.distance);
|
|
}
|
|
|
|
float newDistance =
|
|
Angle::CosineRuleSide(v2.distance, this->distance, rotation);
|
|
|
|
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);
|
|
return vector;
|
|
}
|
|
|
|
Polar Polar::operator-() {
|
|
Polar vector = Polar(this->angle - 180, this->distance);
|
|
return vector;
|
|
}
|
|
|
|
Polar Polar::operator-(const Polar &v2) const {
|
|
Polar vector = *this + (Polar(v2.angle - 180, v2.distance));
|
|
return vector;
|
|
}
|
|
|
|
Polar Polar::operator*(float f) const {
|
|
return Polar(this->angle, this->distance * f);
|
|
}
|
|
|
|
Polar Polar::operator/(const float &f) {
|
|
return Polar(this->angle, this->distance / f);
|
|
}
|
|
|
|
Polar Polar::Rotate(Polar v, float angle) {
|
|
v.angle = Angle::Normalize(v.angle + angle);
|
|
return v;
|
|
} |