128 lines
3.4 KiB
C++
128 lines
3.4 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);
|
|
const Polar Polar::forward = Polar(1.0f, 0.0f);
|
|
const Polar Polar::back = Polar(1.0, 180.0f);
|
|
const Polar Polar::right = Polar(1.0, 90.0f);
|
|
const Polar Polar::left = Polar(1.0, -90.0f);
|
|
|
|
bool Polar::operator==(const Polar &v) const {
|
|
return (this->distance == v.distance && this->angle == v.angle);
|
|
}
|
|
|
|
Polar Polar::Normalize(const Polar &v) {
|
|
Polar r = Polar(1, v.angle);
|
|
return r;
|
|
}
|
|
Polar Polar::normalized() const {
|
|
Polar r = Polar(1, this->angle);
|
|
return r;
|
|
}
|
|
|
|
Polar Polar::operator-() const {
|
|
Polar v = Polar(this->distance, this->angle + 180);
|
|
return v;
|
|
}
|
|
|
|
Polar Polar::operator-(const Polar &v) const {
|
|
Polar r = -v;
|
|
return *this + r;
|
|
}
|
|
Polar Polar::operator-=(const Polar &v) {
|
|
*this = *this - v;
|
|
return *this;
|
|
}
|
|
Polar Polar::operator+(const Polar &v) const {
|
|
if (v.distance == 0)
|
|
return Polar(this->distance, this->angle);
|
|
if (this->distance == 0.0f)
|
|
return v;
|
|
|
|
float deltaAngle = Angle::Normalize(v.angle - (float)this->angle);
|
|
float rotation =
|
|
deltaAngle < 0.0f ? 180.0f + deltaAngle : 180.0f - deltaAngle;
|
|
|
|
if (rotation == 180.0f && v.distance > 0.0f) {
|
|
// angle is too small, take this angle and add the distances
|
|
return Polar(this->distance + v.distance, this->angle);
|
|
}
|
|
|
|
float newDistance =
|
|
Angle::CosineRuleSide(v.distance, this->distance, rotation);
|
|
|
|
float angle = Angle::CosineRuleAngle(newDistance, this->distance, v.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+=(const Polar &v) {
|
|
*this = *this + v;
|
|
return *this;
|
|
}
|
|
|
|
Polar Passer::LinearAlgebra::operator*(const Polar &v, float f) {
|
|
return Polar(v.distance * f, v.angle);
|
|
}
|
|
Polar Passer::LinearAlgebra::operator*(float f, const Polar &v) {
|
|
return Polar(v.distance * f, v.angle);
|
|
}
|
|
Polar Polar::operator*=(float f) {
|
|
this->distance *= f;
|
|
return *this;
|
|
}
|
|
Polar Passer::LinearAlgebra::operator/(const Polar &v, float f) {
|
|
return Polar(v.distance / f, v.angle);
|
|
}
|
|
Polar Passer::LinearAlgebra::operator/(float f, const Polar &v) {
|
|
return Polar(v.distance / f, v.angle);
|
|
}
|
|
Polar Polar::operator/=(float f) {
|
|
this->distance /= f;
|
|
return *this;
|
|
}
|
|
|
|
float Polar::Distance(const Polar &v1, const Polar &v2) {
|
|
float d = Angle::CosineRuleSide(v1.distance, v2.distance,
|
|
(float)v2.angle - (float)v1.angle);
|
|
return d;
|
|
}
|
|
|
|
Polar Polar::Rotate(const Polar &v, Angle angle) {
|
|
Angle a = Angle::Normalize(v.angle + angle);
|
|
Polar r = Polar(v.distance, a);
|
|
return r;
|
|
} |