150 lines
3.8 KiB
C++
150 lines
3.8 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 Angle::Rad2Deg = 57.29578F;
|
|
const float Angle::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;
|
|
}
|
|
|
|
float Angle::Clamp(float angle, float min, float max) {
|
|
float normalizedAngle = Normalize(angle);
|
|
float r = Float::Clamp(normalizedAngle, min, max);
|
|
return r;
|
|
}
|
|
|
|
float Angle::Difference(float a, float b) {
|
|
float r = Normalize(b - a);
|
|
return r;
|
|
}
|
|
|
|
float Angle::MoveTowards(float fromAngle, float toAngle, float maxAngle) {
|
|
float d = toAngle - fromAngle;
|
|
float sign = signbit(d) ? -1 : 1;
|
|
d = sign * Float::Clamp(fabs(d), 0, maxAngle);
|
|
return fromAngle + d;
|
|
}
|
|
|
|
float Angle::CosineRuleSide(float a, float b, float gamma) {
|
|
float a2 = a * a;
|
|
float b2 = b * b;
|
|
float d = a2 + b2 - 2 * a * b * cos(gamma * Angle::Deg2Rad);
|
|
// Catch edge cases where float inacuracies lead tot nans
|
|
if (d < 0)
|
|
return 0;
|
|
|
|
float c = sqrtf(d);
|
|
return c;
|
|
}
|
|
|
|
float Angle::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;
|
|
if (d <= -1)
|
|
return 180;
|
|
|
|
float gamma = acos(d) * Angle::Rad2Deg;
|
|
return gamma;
|
|
}
|
|
|
|
float Angle::SineRuleAngle(float a, float beta, float b) {
|
|
float alpha = asin(a * sin(beta * Angle::Deg2Rad) / b);
|
|
return alpha;
|
|
}
|
|
*/
|
|
//----------------------
|
|
|
|
template <> Angle2<float> Angle2<float>::Rad2Deg = 57.29578F;
|
|
template <> Angle2<float> Angle2<float>::Deg2Rad = 0.0174532924F;
|
|
|
|
template <> Angle2<float> Angle2<float>::Normalize(Angle2<float> angle) {
|
|
float angleValue = angle;
|
|
if (!isfinite(angleValue))
|
|
return angleValue;
|
|
|
|
while (angleValue <= -180)
|
|
angleValue += 360;
|
|
while (angleValue > 180)
|
|
angleValue -= 360;
|
|
return angleValue;
|
|
}
|
|
|
|
template <>
|
|
Angle2<float> Angle2<float>::Clamp(Angle2<float> angle, Angle2<float> min,
|
|
Angle2<float> max) {
|
|
float normalizedAngle = Normalize(angle);
|
|
float r = Float::Clamp(normalizedAngle, min, max);
|
|
return r;
|
|
}
|
|
|
|
// template <typename T>
|
|
// Angle2<T> Angle2<T>::Difference(Angle2<T> a, Angle2<T> b) {
|
|
// Angle2<T> r = Normalize(b - a);
|
|
// return r;
|
|
// }
|
|
|
|
template <>
|
|
Angle2<float> Angle2<float>::MoveTowards(Angle2<float> fromAngle,
|
|
Angle2<float> toAngle,
|
|
Angle2<float> maxAngle) {
|
|
float d = toAngle - fromAngle;
|
|
float sign = signbit(d) ? -1 : 1;
|
|
d = sign * Float::Clamp(fabs(d), 0, maxAngle);
|
|
return fromAngle + d;
|
|
}
|
|
|
|
template <>
|
|
Angle2<float> Angle2<float>::CosineRuleSide(float a, float b,
|
|
Angle2<float> gamma) {
|
|
float a2 = a * a;
|
|
float b2 = b * b;
|
|
float d = a2 + b2 - 2 * a * b * cos(gamma * Angle2<float>::Deg2Rad);
|
|
// Catch edge cases where float inacuracies lead tot nans
|
|
if (d < 0)
|
|
return 0;
|
|
|
|
float c = sqrtf(d);
|
|
return c;
|
|
}
|
|
|
|
template <>
|
|
Angle2<float> Angle2<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;
|
|
if (d <= -1)
|
|
return 180;
|
|
|
|
float gamma = acos(d) * Angle::Rad2Deg;
|
|
return gamma;
|
|
}
|
|
|
|
template <>
|
|
Angle2<float> Angle2<float>::SineRuleAngle(float a, Angle2<float> beta,
|
|
float b) {
|
|
float alpha = asin(a * sin(beta * Angle::Deg2Rad) / b);
|
|
return alpha;
|
|
} |