357 lines
9.4 KiB
C++
357 lines
9.4 KiB
C++
// This Source Code Form is subject to the terms of the Mozilla Public
|
|
// License, v. 2.0.If a copy of the MPL was not distributed with this
|
|
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
|
|
|
|
#include "Angle.h"
|
|
#include "FloatSingle.h"
|
|
#include <math.h>
|
|
|
|
const float Rad2Deg = 57.29578F;
|
|
const float Deg2Rad = 0.0174532924F;
|
|
/*
|
|
float Angle::Normalize(float angle) {
|
|
if (!isfinite(angle))
|
|
return angle;
|
|
|
|
while (angle <= -180)
|
|
angle += 360;
|
|
while (angle > 180)
|
|
angle -= 360;
|
|
return angle;
|
|
}
|
|
*/
|
|
|
|
//----------------------
|
|
|
|
template <typename T> AngleOf<T>::AngleOf() : value(0) {}
|
|
|
|
template <typename T> AngleOf<T>::AngleOf(T angle) : value(angle) {}
|
|
|
|
//===== AngleSingle, AngleOf<float>
|
|
|
|
template <> AngleOf<float> AngleOf<float>::Degrees(float angle) {
|
|
if (isfinite(angle)) {
|
|
while (angle < -180)
|
|
angle += 360;
|
|
while (angle >= 180)
|
|
angle -= 360;
|
|
}
|
|
|
|
return AngleOf(angle);
|
|
}
|
|
|
|
template <> AngleOf<float> AngleOf<float>::Radians(float angle) {
|
|
if (isfinite(angle)) {
|
|
while (angle <= -pi)
|
|
angle += 2 * pi;
|
|
while (angle > pi)
|
|
angle -= 2 * pi;
|
|
}
|
|
|
|
return AngleOf(angle * Rad2Deg);
|
|
}
|
|
|
|
template <typename T> AngleOf<T> AngleOf<T>::Binary(T x) {
|
|
AngleOf<T> angle = AngleOf<T>();
|
|
angle.value = x;
|
|
return angle;
|
|
}
|
|
|
|
template <> float AngleOf<float>::InDegrees() const { return this->value; }
|
|
|
|
template <> float AngleOf<float>::InRadians() const {
|
|
return this->value * Deg2Rad;
|
|
}
|
|
|
|
//===== Angle16, AngleOf<signed short>
|
|
|
|
template <> AngleOf<signed short> AngleOf<signed short>::Degrees(float angle) {
|
|
// map float [-180..180) to integer [-32768..32767]
|
|
signed short value = (signed short)roundf(angle / 360.0F * 65536.0F);
|
|
return AngleOf<signed short>(value);
|
|
}
|
|
|
|
template <> AngleOf<signed short> AngleOf<signed short>::Radians(float angle) {
|
|
if (!isfinite(angle))
|
|
return AngleOf<signed short>(0);
|
|
|
|
// map float [-PI..PI) to integer [-32768..32767]
|
|
signed short value = (signed short)roundf(angle / pi * 32768.0F);
|
|
return AngleOf<signed short>(value);
|
|
}
|
|
|
|
template <> float AngleOf<signed short>::InDegrees() const {
|
|
float degrees = this->value / 65536.0f * 360.0f;
|
|
return degrees;
|
|
}
|
|
|
|
template <> float AngleOf<signed short>::InRadians() const {
|
|
float radians = this->value / 65536.0f * (2 * pi);
|
|
return radians;
|
|
}
|
|
|
|
//===== Angle8, AngleOf<signed char>
|
|
|
|
template <> AngleOf<signed char> AngleOf<signed char>::Degrees(float angle) {
|
|
// map float [-180..180) to integer [-128..127)
|
|
signed char value = (signed char)roundf(angle / 360.0F * 256.0F);
|
|
return AngleOf<signed char>(value);
|
|
}
|
|
|
|
template <> AngleOf<signed char> AngleOf<signed char>::Radians(float angle) {
|
|
if (!isfinite(angle))
|
|
return AngleOf<signed char>(0);
|
|
|
|
// map float [-pi..pi) to integer [-128..127)
|
|
signed char value = (signed char)roundf(angle / pi * 128.0f);
|
|
return AngleOf<signed char>(value);
|
|
}
|
|
|
|
template <> float AngleOf<signed char>::InDegrees() const {
|
|
float degrees = this->value / 256.0f * 360.0f;
|
|
return degrees;
|
|
}
|
|
|
|
template <> float AngleOf<signed char>::InRadians() const {
|
|
float radians = this->value / 128.0f * pi;
|
|
return radians;
|
|
}
|
|
|
|
//===== Generic
|
|
|
|
// template <typename T>
|
|
// const AngleOf<T> AngleOf<T>::zero = AngleOf<T>();
|
|
// template <typename T>
|
|
// const AngleOf<T> AngleOf<T>::deg90 = AngleOf<T>::Degrees(90);
|
|
// template <typename T>
|
|
// const AngleOf<T> AngleOf<T>::deg180 = AngleOf<T>::Degrees(180);
|
|
|
|
template <typename T> bool AngleOf<T>::operator==(const AngleOf<T> a) const {
|
|
return this->value == a.value;
|
|
}
|
|
|
|
template <typename T> bool AngleOf<T>::operator>(AngleOf<T> a) const {
|
|
return this->value > a.value;
|
|
}
|
|
|
|
template <typename T> bool AngleOf<T>::operator>=(AngleOf<T> a) const {
|
|
return this->value >= a.value;
|
|
}
|
|
|
|
template <typename T> bool AngleOf<T>::operator<(AngleOf<T> a) const {
|
|
return this->value < a.value;
|
|
}
|
|
|
|
template <typename T> bool AngleOf<T>::operator<=(AngleOf<T> a) const {
|
|
return this->value <= a.value;
|
|
}
|
|
|
|
template <typename T>
|
|
signed int Passer::LinearAlgebra::AngleOf<T>::Sign(AngleOf<T> a) {
|
|
if (a.value < 0)
|
|
return -1;
|
|
if (a.value > 0)
|
|
return 1;
|
|
return 0;
|
|
}
|
|
|
|
template <typename T>
|
|
AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Abs(AngleOf<T> a) {
|
|
if (Sign(a) < 0)
|
|
return -a;
|
|
else
|
|
return a;
|
|
}
|
|
|
|
template <typename T> AngleOf<T> AngleOf<T>::operator-() const {
|
|
AngleOf<T> angle = AngleOf(-this->value);
|
|
return angle;
|
|
}
|
|
|
|
template <>
|
|
AngleOf<float> AngleOf<float>::operator-(const AngleOf<float> &a) const {
|
|
AngleOf<float> angle = AngleOf(this->value - a.value);
|
|
angle = Normalize(angle);
|
|
return angle;
|
|
}
|
|
template <typename T>
|
|
AngleOf<T> AngleOf<T>::operator-(const AngleOf<T> &a) const {
|
|
AngleOf<T> angle = AngleOf(this->value - a.value);
|
|
return angle;
|
|
}
|
|
|
|
template <>
|
|
AngleOf<float> AngleOf<float>::operator+(const AngleOf<float> &a) const {
|
|
AngleOf<float> angle = AngleOf(this->value + a.value);
|
|
angle = Normalize(angle);
|
|
return angle;
|
|
}
|
|
template <typename T>
|
|
AngleOf<T> AngleOf<T>::operator+(const AngleOf<T> &a) const {
|
|
AngleOf<T> angle = AngleOf(this->value + a.value);
|
|
return angle;
|
|
}
|
|
|
|
template <> AngleOf<float> AngleOf<float>::operator+=(const AngleOf<float> &a) {
|
|
this->value += a.value;
|
|
this->Normalize();
|
|
return *this;
|
|
}
|
|
|
|
template <typename T> AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T> &a) {
|
|
this->value += a.value;
|
|
return *this;
|
|
}
|
|
|
|
template <typename T> void AngleOf<T>::Normalize() {
|
|
float angleValue = this->InDegrees();
|
|
if (!isfinite(angleValue))
|
|
return;
|
|
|
|
while (angleValue <= -180)
|
|
angleValue += 360;
|
|
while (angleValue > 180)
|
|
angleValue -= 360;
|
|
*this = AngleOf::Degrees(angleValue);
|
|
}
|
|
|
|
template <typename T> AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
|
|
float angleValue = angle.InDegrees();
|
|
if (!isfinite(angleValue))
|
|
return angle;
|
|
|
|
while (angleValue <= -180)
|
|
angleValue += 360;
|
|
while (angleValue > 180)
|
|
angleValue -= 360;
|
|
return AngleOf::Degrees(angleValue);
|
|
}
|
|
|
|
template <typename T>
|
|
AngleOf<T> AngleOf<T>::Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max) {
|
|
float r = Float::Clamp(angle.InDegrees(), min.InDegrees(), max.InDegrees());
|
|
return AngleOf<T>::Degrees(r);
|
|
}
|
|
|
|
template <typename T>
|
|
AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
|
|
float maxDegrees) {
|
|
maxDegrees = fmaxf(0, maxDegrees); // filter out negative distances
|
|
AngleOf<T> d = toAngle - fromAngle;
|
|
float dDegrees = Abs(d).InDegrees();
|
|
d = AngleOf<T>::Degrees(Float::Clamp(dDegrees, 0, maxDegrees));
|
|
if (Sign(d) < 0)
|
|
d = -d;
|
|
|
|
return fromAngle + d;
|
|
}
|
|
|
|
template <typename T> float AngleOf<T>::Cos(AngleOf<T> a) {
|
|
return cosf(a.InRadians());
|
|
}
|
|
template <typename T> float AngleOf<T>::Sin(AngleOf<T> a) {
|
|
return sinf(a.InRadians());
|
|
}
|
|
template <typename T> float AngleOf<T>::Tan(AngleOf<T> a) {
|
|
return tanf(a.InRadians());
|
|
}
|
|
|
|
template <typename T> AngleOf<T> AngleOf<T>::Acos(float f) {
|
|
return AngleOf<T>::Radians(acosf(f));
|
|
}
|
|
template <typename T> AngleOf<T> AngleOf<T>::Asin(float f) {
|
|
return AngleOf<T>::Radians(asinf(f));
|
|
}
|
|
template <typename T> AngleOf<T> AngleOf<T>::Atan(float f) {
|
|
return AngleOf<T>::Radians(atanf(f));
|
|
}
|
|
|
|
template <typename T>
|
|
AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Atan2(float f1, float f2) {
|
|
return AngleOf<T>::Radians(atan2f(f1, f2));
|
|
}
|
|
|
|
// template <>
|
|
// float AngleOf<float>::CosineRuleSide(float a, float b, AngleOf<float> gamma)
|
|
// {
|
|
// float a2 = a * a;
|
|
// float b2 = b * b;
|
|
// float d =
|
|
// a2 + b2 -
|
|
// 2 * a * b * Cos(gamma); // cosf(gamma *
|
|
// Passer::LinearAlgebra::Deg2Rad);
|
|
// // Catch edge cases where float inacuracies lead tot nans
|
|
// if (d < 0)
|
|
// return 0.0f;
|
|
|
|
// float c = sqrtf(d);
|
|
// return c;
|
|
// }
|
|
|
|
template <typename T>
|
|
float AngleOf<T>::CosineRuleSide(float a, float b, AngleOf<T> gamma) {
|
|
float a2 = a * a;
|
|
float b2 = b * b;
|
|
float d =
|
|
a2 + b2 -
|
|
2 * a * b * Cos(gamma); // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
|
|
// Catch edge cases where float inacuracies lead tot nans
|
|
if (d < 0)
|
|
return 0;
|
|
|
|
float c = sqrtf(d);
|
|
return c;
|
|
}
|
|
|
|
// template <>
|
|
// AngleOf<float> AngleOf<float>::CosineRuleAngle(float a, float b, float c) {
|
|
// float a2 = a * a;
|
|
// float b2 = b * b;
|
|
// float c2 = c * c;
|
|
// float d = (a2 + b2 - c2) / (2 * a * b);
|
|
// // Catch edge cases where float inacuracies lead tot nans
|
|
// if (d >= 1)
|
|
// return 0.0f;
|
|
// if (d <= -1)
|
|
// return 180.0f;
|
|
|
|
// float gamma = acosf(d) * Rad2Deg;
|
|
// return gamma;
|
|
// }
|
|
template <typename T>
|
|
AngleOf<T> AngleOf<T>::CosineRuleAngle(float a, float b, float c) {
|
|
float a2 = a * a;
|
|
float b2 = b * b;
|
|
float c2 = c * c;
|
|
float d = (a2 + b2 - c2) / (2 * a * b);
|
|
// Catch edge cases where float inacuracies lead tot nans
|
|
if (d >= 1)
|
|
return AngleOf<T>();
|
|
if (d <= -1)
|
|
return AngleOf<T>::Degrees(180);
|
|
|
|
// float gamma = acosf(d) * Rad2Deg;
|
|
AngleOf<T> gamma = Acos(d);
|
|
return gamma;
|
|
}
|
|
|
|
// template <>
|
|
// AngleOf<float> AngleOf<float>::SineRuleAngle(float a,
|
|
// AngleOf<float> beta,
|
|
// float b) {
|
|
// float deg2rad = Deg2Rad;
|
|
// float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b);
|
|
// return alpha;
|
|
// }
|
|
template <typename T>
|
|
AngleOf<T> AngleOf<T>::SineRuleAngle(float a, AngleOf<T> beta, float b) {
|
|
// float deg2rad = Deg2Rad;
|
|
// float alpha = asinf(a * sinf(beta.InDegrees() * deg2rad) / b);
|
|
AngleOf<T> alpha = Asin(a * Sin(beta) / b);
|
|
return alpha;
|
|
}
|
|
|
|
template class AngleOf<float>;
|
|
template class AngleOf<signed char>;
|
|
template class AngleOf<signed short>;
|