513 lines
17 KiB
C++
513 lines
17 KiB
C++
#include "Spherical.h"
|
|
|
|
#include "Angle.h"
|
|
#include "Quaternion.h"
|
|
|
|
#include <math.h>
|
|
|
|
template <typename T>
|
|
SphericalOf<T>::SphericalOf() {
|
|
this->distance = 0.0f;
|
|
this->horizontal = AngleOf<T>();
|
|
this->vertical = AngleOf<T>();
|
|
}
|
|
|
|
// template <>
|
|
// SphericalOf<signed short>::SphericalOf() {
|
|
// this->distance = 0.0f;
|
|
// this->horizontal = AngleOf<signed short>(0);
|
|
// this->vertical = AngleOf<signed short>(0);
|
|
// }
|
|
|
|
template <typename T>
|
|
SphericalOf<T>::SphericalOf(float distance,
|
|
AngleOf<T> horizontal,
|
|
AngleOf<T> vertical) {
|
|
this->distance = distance;
|
|
this->horizontal = horizontal;
|
|
this->vertical = vertical;
|
|
}
|
|
|
|
// template <>
|
|
// SphericalOf<float>::SphericalOf(float distance,
|
|
// AngleOf<float> horizontal,
|
|
// AngleOf<float> vertical) {
|
|
// this->distance = distance;
|
|
// this->horizontal = horizontal;
|
|
// this->vertical = vertical;
|
|
// }
|
|
|
|
// template <>
|
|
// SphericalOf<signed short>::SphericalOf(float distance,
|
|
// AngleOf<signed short> horizontal,
|
|
// AngleOf<signed short> vertical) {
|
|
// this->distance = distance;
|
|
// this->horizontal = horizontal;
|
|
// this->vertical = vertical;
|
|
// }
|
|
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
|
|
AngleOf<T> horizontal = polar.angle;
|
|
AngleOf<T> vertical = AngleOf<T>();
|
|
SphericalOf<T> r = SphericalOf(polar.distance, horizontal, vertical);
|
|
return r;
|
|
}
|
|
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
|
|
float distance = v.magnitude();
|
|
if (distance == 0.0f) {
|
|
return SphericalOf(distance, AngleOf<T>(), AngleOf<T>());
|
|
} else {
|
|
AngleOf<T> verticalAngle =
|
|
AngleOf<T>::Radians((90.0f - acosf(v.Up() / distance)));
|
|
AngleOf<T> horizontalAngle =
|
|
AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
|
|
return SphericalOf(distance, horizontalAngle, verticalAngle);
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
Vector3 SphericalOf<T>::ToVector3() const {
|
|
float verticalRad = (pi / 2) - this->vertical.InRadians();
|
|
float horizontalRad = this->horizontal.InRadians();
|
|
|
|
float cosVertical = cosf(verticalRad);
|
|
float sinVertical = sinf(verticalRad);
|
|
float cosHorizontal = cosf(horizontalRad);
|
|
float sinHorizontal = sinf(horizontalRad);
|
|
|
|
float x = this->distance * sinVertical * sinHorizontal;
|
|
float y = this->distance * cosVertical;
|
|
float z = this->distance * sinVertical * cosHorizontal;
|
|
|
|
Vector3 v = Vector3(x, y, z);
|
|
return v;
|
|
}
|
|
|
|
template <typename T>
|
|
const SphericalOf<T> SphericalOf<T>::zero =
|
|
SphericalOf<T>(0.0f, AngleOf<T>(), AngleOf<T>());
|
|
template <typename T>
|
|
const SphericalOf<T> SphericalOf<T>::forward =
|
|
SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>());
|
|
template <typename T>
|
|
const SphericalOf<T> SphericalOf<T>::back =
|
|
SphericalOf<T>(1.0f, AngleOf<T>::Degrees(180), AngleOf<T>());
|
|
template <typename T>
|
|
const SphericalOf<T> SphericalOf<T>::right =
|
|
SphericalOf<T>(1.0f, AngleOf<T>::Degrees(90), AngleOf<T>());
|
|
template <typename T>
|
|
const SphericalOf<T> SphericalOf<T>::left =
|
|
SphericalOf<T>(1.0f, AngleOf<T>::Degrees(-90), AngleOf<T>());
|
|
template <typename T>
|
|
const SphericalOf<T> SphericalOf<T>::up =
|
|
SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>::Degrees(90));
|
|
template <typename T>
|
|
const SphericalOf<T> SphericalOf<T>::down =
|
|
SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>::Degrees(-90));
|
|
|
|
template <>
|
|
const SphericalOf<signed short> SphericalOf<signed short>::zero =
|
|
SphericalOf(0.0f, AngleOf<signed short>(), AngleOf<signed short>());
|
|
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
|
|
SphericalOf<T> v = SphericalOf<T>(distance, this->horizontal, this->vertical);
|
|
return SphericalOf<T>();
|
|
}
|
|
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::operator-() const {
|
|
SphericalOf<T> v = SphericalOf<T>(this->distance,
|
|
this->horizontal + AngleOf<T>::Degrees(180),
|
|
this->vertical + AngleOf<T>::Degrees(180));
|
|
return v;
|
|
}
|
|
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const {
|
|
// let's do it the easy way...
|
|
Vector3 v1 = this->ToVector3();
|
|
Vector3 v2 = s2.ToVector3();
|
|
Vector3 v = v1 - v2;
|
|
SphericalOf<T> r = SphericalOf<T>::FromVector3(v);
|
|
return r;
|
|
}
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T>& v) {
|
|
*this = *this - v;
|
|
return *this;
|
|
}
|
|
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const {
|
|
// let's do it the easy way...
|
|
Vector3 v1 = this->ToVector3();
|
|
Vector3 v2 = s2.ToVector3();
|
|
Vector3 v = v1 + v2;
|
|
SphericalOf<T> r = SphericalOf<T>::FromVector3(v);
|
|
return r;
|
|
/*
|
|
// This is the hard way...
|
|
if (v2.distance <= 0)
|
|
return Spherical(this->distance, this->horizontalAngle,
|
|
this->verticalAngle);
|
|
if (this->distance <= 0)
|
|
return v2;
|
|
|
|
float deltaHorizontalAngle =
|
|
(float)Angle::Normalize(v2.horizontalAngle - this->horizontalAngle);
|
|
float horizontalRotation = deltaHorizontalAngle < 0
|
|
? 180 + deltaHorizontalAngle
|
|
: 180 - deltaHorizontalAngle;
|
|
float deltaVerticalAngle =
|
|
Angle::Normalize(v2.verticalAngle - this->verticalAngle);
|
|
float verticalRotation = deltaVerticalAngle < 0 ? 180 + deltaVerticalAngle
|
|
: 180 - deltaVerticalAngle;
|
|
|
|
if (horizontalRotation == 180 && verticalRotation == 180)
|
|
// angle is too small, take this angle and add the distances
|
|
return Spherical(this->distance + v2.distance, this->horizontalAngle,
|
|
this->verticalAngle);
|
|
|
|
Angle rotation = AngleBetween(*this, v2);
|
|
float newDistance =
|
|
Angle::CosineRuleSide(v2.distance, this->distance, rotation);
|
|
float angle =
|
|
Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
|
|
|
|
// Now we have to project the angle to the horizontal and vertical planes...
|
|
// The axis for the angle is the cross product of the two spherical vectors
|
|
// (which function we do not have either...)
|
|
float horizontalAngle = 0;
|
|
float verticalAngle = 0;
|
|
|
|
float newHorizontalAngle =
|
|
deltaHorizontalAngle < 0
|
|
? Angle::Normalize(this->horizontalAngle - horizontalAngle)
|
|
: Angle::Normalize(this->horizontalAngle + horizontalAngle);
|
|
float newVerticalAngle =
|
|
deltaVerticalAngle < 0
|
|
? Angle::Normalize(this->verticalAngle - verticalAngle)
|
|
: Angle::Normalize(this->verticalAngle + verticalAngle);
|
|
|
|
Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle);
|
|
*/
|
|
}
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T>& v) {
|
|
*this = *this + v;
|
|
return *this;
|
|
}
|
|
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::operator*=(float f) {
|
|
this->distance *= f;
|
|
return *this;
|
|
}
|
|
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::operator/=(float f) {
|
|
this->distance /= f;
|
|
return *this;
|
|
}
|
|
|
|
#include "FloatSingle.h"
|
|
#include "Vector3.h"
|
|
|
|
const float epsilon = 1E-05f;
|
|
|
|
template <typename T>
|
|
float SphericalOf<T>::DistanceBetween(const SphericalOf<T>& v1,
|
|
const SphericalOf<T>& v2) {
|
|
// SphericalOf<T> difference = v1 - v2;
|
|
// return difference.distance;
|
|
Vector3 vec1 = v1.ToVector3();
|
|
Vector3 vec2 = v2.ToVector3();
|
|
float distance = Vector3::Distance(vec1, vec2);
|
|
return distance;
|
|
}
|
|
|
|
template <typename T>
|
|
AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
|
|
const SphericalOf& v2) {
|
|
// float denominator = v1.distance * v2.distance;
|
|
// if (denominator < epsilon)
|
|
// return 0.0f;
|
|
|
|
Vector3 v1_3 = v1.ToVector3();
|
|
Vector3 v2_3 = v2.ToVector3();
|
|
// float dot = Vector3::Dot(v1_3, v2_3);
|
|
// float fraction = dot / denominator;
|
|
// if (isnan(fraction))
|
|
// return fraction; // short cut to returning NaN universally
|
|
|
|
// float cdot = Float::Clamp(fraction, -1.0, 1.0);
|
|
// float r = ((float)acos(cdot)) * Rad2Deg;
|
|
Angle r = Vector3::Angle(v1_3, v2_3);
|
|
return AngleOf<T>::Degrees(r.InDegrees());
|
|
}
|
|
|
|
template <typename T>
|
|
AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
|
|
const SphericalOf<T>& v1,
|
|
const SphericalOf<T>& v2,
|
|
const SphericalOf<T>& axis) {
|
|
Vector3 v1_vector = v1.ToVector3();
|
|
Vector3 v2_vector = v2.ToVector3();
|
|
Vector3 axis_vector = axis.ToVector3();
|
|
Angle r = Vector3::SignedAngle(v1_vector, v2_vector, axis_vector);
|
|
return AngleOf<T>::Degrees(r.InDegrees());
|
|
}
|
|
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
|
|
AngleOf<T> horizontalAngle,
|
|
AngleOf<T> verticalAngle) {
|
|
SphericalOf<T> r = SphericalOf(v.distance, v.horizontal + horizontalAngle,
|
|
v.vertical + verticalAngle);
|
|
return r;
|
|
}
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T>& v,
|
|
AngleOf<T> a) {
|
|
SphericalOf<T> r = SphericalOf(v.distance, v.horizontal + a, v.vertical);
|
|
return r;
|
|
}
|
|
template <typename T>
|
|
SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
|
|
AngleOf<T> a) {
|
|
SphericalOf<T> r = SphericalOf(v.distance, v.horizontal, v.vertical + a);
|
|
return r;
|
|
}
|
|
|
|
template class SphericalOf<float>;
|
|
template class SphericalOf<signed short>;
|
|
|
|
//---------------------------------------
|
|
/*
|
|
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.ToFloat() - 180.0f);
|
|
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) *
|
|
Passer::LinearAlgebra::Rad2Deg);
|
|
this->horizontalAngle =
|
|
atan2f(v.Right(), v.Forward()) * Passer::LinearAlgebra::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.ToFloat() == v.horizontalAngle.ToFloat() &&
|
|
this->verticalAngle.ToFloat() == v.verticalAngle.ToFloat());
|
|
}
|
|
|
|
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.ToFloat() + 180.0f,
|
|
this->verticalAngle.ToFloat() + 180.0f);
|
|
return v;
|
|
}
|
|
|
|
Spherical Spherical::operator-(const Spherical& s2) const {
|
|
// let's do it the easy way...
|
|
Vector3 v1 = Vector3(*this);
|
|
Vector3 v2 = Vector3(s2);
|
|
Vector3 v = v1 - v2;
|
|
Spherical r = Spherical(v);
|
|
return r;
|
|
}
|
|
Spherical Spherical::operator-=(const Spherical& v) {
|
|
*this = *this - v;
|
|
return *this;
|
|
}
|
|
|
|
Spherical Spherical::operator+(const Spherical& s2) const {
|
|
// let's do it the easy way...
|
|
Vector3 v1 = Vector3(*this);
|
|
Vector3 v2 = Vector3(s2);
|
|
Vector3 v = v1 + v2;
|
|
Spherical r = Spherical(v);
|
|
return r;
|
|
|
|
// This is the hard way...
|
|
// if (v2.distance <= 0)
|
|
// return Spherical(this->distance, this->horizontalAngle,
|
|
// this->verticalAngle);
|
|
// if (this->distance <= 0)
|
|
// return v2;
|
|
|
|
// float deltaHorizontalAngle =
|
|
// (float)Angle::Normalize(v2.horizontalAngle - this->horizontalAngle);
|
|
// float horizontalRotation = deltaHorizontalAngle < 0
|
|
// ? 180 + deltaHorizontalAngle
|
|
// : 180 - deltaHorizontalAngle;
|
|
// float deltaVerticalAngle =
|
|
// Angle::Normalize(v2.verticalAngle - this->verticalAngle);
|
|
// float verticalRotation = deltaVerticalAngle < 0 ? 180 + deltaVerticalAngle
|
|
// : 180 - deltaVerticalAngle;
|
|
|
|
// if (horizontalRotation == 180 && verticalRotation == 180)
|
|
// // angle is too small, take this angle and add the distances
|
|
// return Spherical(this->distance + v2.distance, this->horizontalAngle,
|
|
// this->verticalAngle);
|
|
|
|
// Angle rotation = AngleBetween(*this, v2);
|
|
// float newDistance =
|
|
// Angle::CosineRuleSide(v2.distance, this->distance, rotation);
|
|
// float angle =
|
|
// Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
|
|
|
|
// // Now we have to project the angle to the horizontal and vertical
|
|
planes...
|
|
// // The axis for the angle is the cross product of the two spherical vectors
|
|
// // (which function we do not have either...)
|
|
// float horizontalAngle = 0;
|
|
// float verticalAngle = 0;
|
|
|
|
// float newHorizontalAngle =
|
|
// deltaHorizontalAngle < 0
|
|
// ? Angle::Normalize(this->horizontalAngle - horizontalAngle)
|
|
// : Angle::Normalize(this->horizontalAngle + horizontalAngle);
|
|
// float newVerticalAngle =
|
|
// deltaVerticalAngle < 0
|
|
// ? Angle::Normalize(this->verticalAngle - verticalAngle)
|
|
// : Angle::Normalize(this->verticalAngle + verticalAngle);
|
|
|
|
// Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle);
|
|
|
|
}
|
|
Spherical Spherical::operator+=(const Spherical& v) {
|
|
*this = *this + v;
|
|
return *this;
|
|
}
|
|
|
|
// Spherical Passer::LinearAlgebra::operator*(const Spherical &v, float f) {
|
|
// return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
|
|
// }
|
|
// Spherical Passer::LinearAlgebra::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::LinearAlgebra::operator/(const Spherical &v, float f) {
|
|
// return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
|
|
// }
|
|
// Spherical Passer::LinearAlgebra::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;
|
|
// }
|
|
|
|
#include "AngleUsing.h"
|
|
#include "FloatSingle.h"
|
|
#include "Vector3.h"
|
|
|
|
const float epsilon = 1E-05f;
|
|
const float Rad2Deg = 57.29578F;
|
|
|
|
Angle Spherical::AngleBetween(const Spherical& v1, const Spherical& v2) {
|
|
// float denominator = sqrtf(v1_3.sqrMagnitude() * v2_3.sqrMagnitude());
|
|
float denominator =
|
|
v1.distance * v2.distance; // sqrtf(v1.distance * v1.distance *
|
|
// v2.distance * v2.distance);
|
|
if (denominator < epsilon)
|
|
return 0.0f;
|
|
|
|
Vector3 v1_3 = Vector3(v1);
|
|
Vector3 v2_3 = Vector3(v2);
|
|
float dot = Vector3::Dot(v1_3, v2_3);
|
|
float fraction = dot / denominator;
|
|
if (isnan(fraction))
|
|
return fraction; // short cut to returning NaN universally
|
|
|
|
float cdot = Float::Clamp(fraction, -1.0, 1.0);
|
|
float r = ((float)acos(cdot)) * Rad2Deg;
|
|
return r;
|
|
}
|
|
|
|
Spherical Spherical::Rotate(const Spherical& v,
|
|
Angle horizontalAngle,
|
|
Angle verticalAngle) {
|
|
Spherical r = Spherical(
|
|
v.distance, v.horizontalAngle.ToFloat() + horizontalAngle.ToFloat(),
|
|
v.verticalAngle.ToFloat() + verticalAngle.ToFloat());
|
|
return r;
|
|
}
|
|
Spherical Spherical::RotateHorizontal(const Spherical& v, Angle a) {
|
|
Spherical r = Spherical(v.distance, v.horizontalAngle.ToFloat() + a.ToFloat(),
|
|
v.verticalAngle.ToFloat());
|
|
return r;
|
|
}
|
|
Spherical Spherical::RotateVertical(const Spherical& v, Angle a) {
|
|
Spherical r = Spherical(v.distance, v.horizontalAngle.ToFloat(),
|
|
v.verticalAngle.ToFloat() + a.ToFloat());
|
|
return r;
|
|
}
|
|
*/
|