101 lines
2.8 KiB
C++
101 lines
2.8 KiB
C++
#include <math.h>
|
|
|
|
#include "Angle.h"
|
|
#include "Polar.h"
|
|
#include "Spherical.h"
|
|
|
|
Polar::Polar() {
|
|
angle = 0.0F;
|
|
distance = 0.0F;
|
|
}
|
|
|
|
// Polar::Polar(float newAngle, float newDistance) {
|
|
Polar::Polar(float newDistance, Angle newAngle) {
|
|
// distance should always be 0 or greater
|
|
if (newDistance < 0) {
|
|
angle = Angle::Normalize(newAngle - 180);
|
|
distance = -newDistance;
|
|
} else {
|
|
angle = Angle::Normalize(newAngle);
|
|
distance = newDistance;
|
|
}
|
|
}
|
|
|
|
Polar::Polar(Vector2 v) {
|
|
angle = Vector2::SignedAngle(
|
|
Vector2::forward,
|
|
v); // atan2(v.x, sqrt(v.z * v.z + v.y * v.y)) * Angle::Rad2Deg;
|
|
distance = v.magnitude();
|
|
}
|
|
|
|
Polar::Polar(Spherical s) {
|
|
angle = s.horizontalAngle;
|
|
distance = s.distance * cosf(s.verticalAngle * Angle::Deg2Rad);
|
|
}
|
|
|
|
const Polar Polar::zero = Polar(0, 0);
|
|
|
|
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::operator+(Polar &v2) {
|
|
if (v2.distance == 0)
|
|
return Polar(this->distance,
|
|
this->angle); // Polar(this->angle, this->distance);
|
|
if (this->distance == 0)
|
|
return v2;
|
|
|
|
float deltaAngle = Angle::Normalize(v2.angle - (float)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->distance + v2.distance,
|
|
this->angle); // 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 ? (float)this->angle - angle : (float)this->angle + angle;
|
|
newAngle = Angle::Normalize(newAngle);
|
|
Polar vector = Polar(newDistance, newAngle); // Polar(newAngle, newDistance);
|
|
return vector;
|
|
}
|
|
|
|
Polar Polar::operator-() {
|
|
Polar vector = Polar(this->distance,
|
|
(float)this->angle -
|
|
180); // Polar(this->angle - 180, this->distance);
|
|
return vector;
|
|
}
|
|
|
|
Polar Polar::operator-(Polar &v2) {
|
|
// Polar vector = *this + Polar(v2.distance, (float)v2.angle - 180);
|
|
//(Polar(v2.angle - 180, v2.distance));
|
|
Polar vector = -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);
|
|
}
|
|
|
|
Polar Polar::Rotate(Polar v, Angle angle) {
|
|
v.angle = Angle::Normalize(v.angle + angle);
|
|
return v;
|
|
} |