102 lines
2.7 KiB
C++
102 lines
2.7 KiB
C++
#include <math.h>
|
|
|
|
#include "Angle.h"
|
|
#include "Polar.h"
|
|
#include "Spherical.h"
|
|
|
|
Polar::Polar() {
|
|
this->distance = 0.0f;
|
|
this->angle = 0.0f;
|
|
}
|
|
|
|
Polar::Polar(float distance, Angle angle) {
|
|
// distance should always be 0 or greater
|
|
if (distance < 0.0f) {
|
|
this->distance = -distance;
|
|
this->angle = Angle::Normalize(angle - 180);
|
|
} else {
|
|
this->distance = distance;
|
|
if (this->distance == 0.0f)
|
|
// angle is always 0 if distance is 0
|
|
this->angle = 0.0f;
|
|
else
|
|
this->angle = Angle::Normalize(angle);
|
|
}
|
|
}
|
|
|
|
Polar::Polar(Vector2 v) {
|
|
this->distance = v.magnitude();
|
|
this->angle = Vector2::SignedAngle(Vector2::forward, v);
|
|
}
|
|
|
|
Polar::Polar(Spherical v) {
|
|
this->distance = v.distance * cosf(v.verticalAngle * Angle::Deg2Rad);
|
|
this->angle = v.horizontalAngle;
|
|
}
|
|
|
|
const Polar Polar::zero = Polar(0.0f, 0.0f);
|
|
|
|
bool Polar::operator==(const Polar &v) {
|
|
return (this->distance == v.distance && this->angle == v.angle);
|
|
}
|
|
|
|
Polar Polar::operator+(Polar &v2) {
|
|
if (v2.distance == 0)
|
|
return Polar(this->distance, this->angle);
|
|
if (this->distance == 0.0f)
|
|
return v2;
|
|
|
|
float deltaAngle = Angle::Normalize(v2.angle - (float)this->angle);
|
|
float rotation =
|
|
deltaAngle < 0.0f ? 180.0f + deltaAngle : 180.0f - deltaAngle;
|
|
|
|
if (rotation == 180.0f && v2.distance > 0.0f) {
|
|
// angle is too small, take this angle and add the distances
|
|
return Polar(this->distance + v2.distance, this->angle);
|
|
}
|
|
|
|
float newDistance =
|
|
Angle::CosineRuleSide(v2.distance, this->distance, rotation);
|
|
|
|
float angle =
|
|
Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
|
|
|
|
float newAngle = deltaAngle < 0.0f ? (float)this->angle - angle
|
|
: (float)this->angle + angle;
|
|
newAngle = Angle::Normalize(newAngle);
|
|
Polar vector = Polar(newDistance, newAngle);
|
|
return vector;
|
|
}
|
|
|
|
Polar Polar::operator-() {
|
|
Polar v = Polar(this->distance, this->angle + 180);
|
|
return v;
|
|
}
|
|
|
|
Polar Polar::operator-(Polar &v2) {
|
|
// Polar vector = *this + Polar(v2.distance, (float)v2.angle - 180);
|
|
//(Polar(v2.angle - 180, v2.distance));
|
|
v2 = -v2;
|
|
return *this + v2;
|
|
}
|
|
|
|
Polar Polar::operator*(float f) const {
|
|
return Polar(this->distance * f,
|
|
this->angle); // Polar(this->angle, this->distance * f);
|
|
}
|
|
|
|
Polar Polar::operator/(const float &f) {
|
|
return Polar(this->distance / f,
|
|
this->angle); // Polar(this->angle, this->distance / f);
|
|
}
|
|
|
|
float Polar::Distance(Polar &v1, Polar &v2) {
|
|
float d = Angle::CosineRuleSide(v1.distance, v2.distance,
|
|
(float)v2.angle - (float)v1.angle);
|
|
return d;
|
|
}
|
|
|
|
Polar Polar::Rotate(Polar v, Angle angle) {
|
|
v.angle = Angle::Normalize(v.angle + angle);
|
|
return v;
|
|
} |