63 lines
1.9 KiB
C++
63 lines
1.9 KiB
C++
#include "Spherical.h"
|
|
|
|
#include "Angle.h"
|
|
#include "Quaternion.h"
|
|
|
|
#include <math.h>
|
|
|
|
// using Angle = float;
|
|
|
|
Spherical::Spherical() {
|
|
this->horizontalAngle = 0;
|
|
this->verticalAngle = 0;
|
|
this->distance = 0;
|
|
}
|
|
|
|
// Spherical::Spherical(float polarAngle, float elevationAngle, float distance)
|
|
// {
|
|
// this->horizontalAngle = polarAngle;
|
|
// this->verticalAngle = elevationAngle;
|
|
// this->distance = distance;
|
|
// }
|
|
|
|
Spherical::Spherical(Polar polar) {
|
|
this->horizontalAngle = polar.angle;
|
|
this->verticalAngle = 0.0F;
|
|
this->distance = polar.distance;
|
|
}
|
|
|
|
Spherical::Spherical(float distance, Angle horizontalAngle, Angle verticalAngle)
|
|
: distance(distance), horizontalAngle(horizontalAngle),
|
|
verticalAngle(verticalAngle) {}
|
|
|
|
Spherical::Spherical(Vector3 v) {
|
|
float signZ = (v.z >= 0) - (v.z < 0);
|
|
verticalAngle =
|
|
atan2(v.y, signZ * sqrt(v.z * v.z + v.x * v.x)) * Angle::Rad2Deg;
|
|
horizontalAngle = atan2(v.x, sqrt(v.z * v.z + v.y * v.y)) * Angle::Rad2Deg;
|
|
distance = v.magnitude();
|
|
}
|
|
|
|
const Spherical Spherical::zero = Spherical(0.0F, (Angle)0.0F, (Angle)0.0F);
|
|
|
|
float Spherical::GetSwing() {
|
|
// Not sure if this is correct
|
|
return sqrtf(horizontalAngle * horizontalAngle +
|
|
verticalAngle * verticalAngle);
|
|
}
|
|
|
|
Polar Spherical::ProjectOnHorizontalPlane() {
|
|
return Polar(horizontalAngle, distance);
|
|
}
|
|
|
|
Vector3 Spherical::ToVector3() {
|
|
// Vector3 v = Quaternion::AngleAxis(this->horizontalAngle, Vector3::up) *
|
|
// Quaternion::AngleAxis(this->verticalAngle, Vector3::left) *
|
|
// Vector3::forward * this->distance;
|
|
float verticalRad = verticalAngle * Angle::Deg2Rad;
|
|
float horizontalRad = horizontalAngle * Angle::Deg2Rad;
|
|
Vector3 v = Vector3(this->distance * cosf(verticalRad) * sinf(horizontalRad),
|
|
this->distance * sinf(verticalRad),
|
|
this->distance * cosf(verticalRad) * cosf(horizontalRad));
|
|
return v;
|
|
} |