100 lines
2.7 KiB
C++
100 lines
2.7 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(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->distance,
|
|
this->angle); // 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->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 ? Angle::Normalize(this->angle - angle)
|
|
: Angle::Normalize(this->angle + angle);
|
|
Polar vector = Polar(newDistance, newAngle); // Polar(newAngle, newDistance);
|
|
return vector;
|
|
}
|
|
|
|
Polar Polar::operator-() {
|
|
Polar vector =
|
|
Polar(this->distance,
|
|
this->angle - 180); // Polar(this->angle - 180, this->distance);
|
|
return vector;
|
|
}
|
|
|
|
Polar Polar::operator-(const Polar &v2) const {
|
|
Polar vector =
|
|
*this + Polar(v2.distance,
|
|
v2.angle - 180); //(Polar(v2.angle - 180, v2.distance));
|
|
return vector;
|
|
}
|
|
|
|
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, float angle) {
|
|
v.angle = Angle::Normalize(v.angle + angle);
|
|
return v;
|
|
} |