119 lines
3.7 KiB
C++
119 lines
3.7 KiB
C++
#include "Spherical.h"
|
|
|
|
#include "Angle.h"
|
|
#include "Quaternion.h"
|
|
|
|
#include <math.h>
|
|
|
|
Spherical::Spherical() {
|
|
this->distance = 0.0f;
|
|
this->horizontalAngle = 0.0f;
|
|
this->verticalAngle = 0.0f;
|
|
}
|
|
|
|
Spherical::Spherical(Polar polar) {
|
|
this->distance = polar.distance;
|
|
this->horizontalAngle = polar.angle;
|
|
this->verticalAngle = 0.0f;
|
|
}
|
|
|
|
Spherical::Spherical(float distance, Angle horizontalAngle,
|
|
Angle verticalAngle) {
|
|
if (distance < 0) {
|
|
this->distance = -distance;
|
|
this->horizontalAngle = Angle::Normalize(horizontalAngle - 180);
|
|
this->verticalAngle = verticalAngle;
|
|
} else {
|
|
this->distance = distance;
|
|
this->horizontalAngle = Angle::Normalize(horizontalAngle);
|
|
this->verticalAngle = Angle::Normalize(verticalAngle);
|
|
}
|
|
}
|
|
|
|
Spherical::Spherical(Vector3 v) {
|
|
this->distance = v.magnitude();
|
|
if (distance == 0.0f) {
|
|
this->verticalAngle = 0.0f;
|
|
this->horizontalAngle = 0.0f;
|
|
} else {
|
|
this->verticalAngle =
|
|
(90.0f - acosf(v.Up() / this->distance) * Angle::Rad2Deg);
|
|
this->horizontalAngle = atan2f(v.Right(), v.Forward()) * Angle::Rad2Deg;
|
|
}
|
|
}
|
|
|
|
const Spherical Spherical::zero = Spherical(0.0f, 0.0f, 0.0f);
|
|
const Spherical Spherical::forward = Spherical(1.0f, 0.0f, 0.0f);
|
|
const Spherical Spherical::back = Spherical(1.0f, 180.0f, 0.0f);
|
|
const Spherical Spherical::right = Spherical(1.0f, 90.0f, 0.0f);
|
|
const Spherical Spherical::left = Spherical(1.0f, -90.0f, 0.0f);
|
|
const Spherical Spherical::up = Spherical(1.0f, 0.0f, 90.0f);
|
|
const Spherical Spherical::down = Spherical(1.0f, 0.0f, -90.0f);
|
|
|
|
bool Spherical::operator==(const Spherical &v) const {
|
|
return (this->distance == v.distance &&
|
|
this->horizontalAngle == v.horizontalAngle &&
|
|
this->verticalAngle == v.verticalAngle);
|
|
}
|
|
|
|
Spherical Spherical::Normalize(const Spherical &v) {
|
|
Spherical r = Spherical(1, v.horizontalAngle, v.verticalAngle);
|
|
return r;
|
|
}
|
|
Spherical Spherical::normalized() const {
|
|
Spherical r = Spherical(1, this->horizontalAngle, this->verticalAngle);
|
|
return r;
|
|
}
|
|
|
|
Spherical Spherical::operator-() const {
|
|
Spherical v = Spherical(this->distance, this->horizontalAngle + 180.0f,
|
|
this->verticalAngle + 180.0f);
|
|
return v;
|
|
}
|
|
|
|
Spherical Passer::operator*(const Spherical &v, float f) {
|
|
return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
|
|
}
|
|
Spherical Passer::operator*(float f, const Spherical &v) {
|
|
return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
|
|
}
|
|
Spherical Spherical::operator*=(float f) {
|
|
this->distance *= f;
|
|
return *this;
|
|
}
|
|
Spherical Passer::operator/(const Spherical &v, float f) {
|
|
return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
|
|
}
|
|
Spherical Passer::operator/(float f, const Spherical &v) {
|
|
return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
|
|
}
|
|
Spherical Spherical::operator/=(float f) {
|
|
this->distance /= f;
|
|
return *this;
|
|
}
|
|
|
|
// float Spherical::GetSwing() {
|
|
// // Not sure if this is correct
|
|
// return sqrtf(horizontalAngle * horizontalAngle +
|
|
// verticalAngle * verticalAngle);
|
|
// }
|
|
|
|
// float Spherical::Distance(const Spherical &s1, const Spherical &s2) {
|
|
// float d = 0;
|
|
// return d;
|
|
// }
|
|
|
|
Spherical Spherical::Rotate(const Spherical &v, Angle horizontalAngle,
|
|
Angle verticalAngle) {
|
|
Spherical r = Spherical(v.distance, v.horizontalAngle + horizontalAngle,
|
|
v.verticalAngle + verticalAngle);
|
|
return r;
|
|
}
|
|
Spherical Spherical::RotateHorizontal(const Spherical &v, Angle a) {
|
|
Spherical r = Spherical(v.distance, v.horizontalAngle + a, v.verticalAngle);
|
|
return r;
|
|
}
|
|
Spherical Spherical::RotateVertical(const Spherical &v, Angle a) {
|
|
Spherical r = Spherical(v.distance, v.horizontalAngle, v.verticalAngle + a);
|
|
return r;
|
|
} |