RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add Binary support

Browse Source
This commit is contained in:
Pascal Serrarens 2024-11-29 12:56:46 +01:00
parent 18756ba1a5
commit 536d6cef19
2 changed files with 56 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
Angle.cpp
View File

@ -3,8 +3,8 @@
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include "Angle.h"
#include <math.h>
#include "FloatSingle.h"
#include <math.h>
const float Rad2Deg = 57.29578F;
const float Deg2Rad = 0.0174532924F;
@ -23,16 +23,13 @@ float Angle::Normalize(float angle) {
//----------------------
template <typename T>
AngleOf<T>::AngleOf() : value(0) {}
template <typename T> AngleOf<T>::AngleOf() : value(0) {}
template <typename T>
AngleOf<T>::AngleOf(T angle) : value(angle) {}
template <typename T> AngleOf<T>::AngleOf(T angle) : value(angle) {}
//===== AngleSingle, AngleOf<float>
template <>
AngleOf<float> AngleOf<float>::Degrees(float angle) {
template <> AngleOf<float> AngleOf<float>::Degrees(float angle) {
if (isfinite(angle)) {
while (angle < -180)
angle += 360;
@ -43,8 +40,7 @@ AngleOf<float> AngleOf<float>::Degrees(float angle) {
return AngleOf(angle);
}
template <>
AngleOf<float> AngleOf<float>::Radians(float angle) {
template <> AngleOf<float> AngleOf<float>::Radians(float angle) {
if (isfinite(angle)) {
while (angle <= -pi)
angle += 2 * pi;
@ -55,27 +51,27 @@ AngleOf<float> AngleOf<float>::Radians(float angle) {
return AngleOf(angle * Rad2Deg);
}
template <>
float AngleOf<float>::InDegrees() const {
return this->value;
template <typename T> AngleOf<T> AngleOf<T>::Binary(T x) {
AngleOf<T> angle = AngleOf<T>();
angle.value = x;
return angle;
}
template <>
float AngleOf<float>::InRadians() const {
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) {
template <> AngleOf<signed short> AngleOf<signed short>::Degrees(float angle) {
// map float [-180..180) to integer [-32768..32767]
signed short value = (signed short)(angle / 360.0F * 65536.0F);
return AngleOf<signed short>(value);
}
template <>
AngleOf<signed short> AngleOf<signed short>::Radians(float angle) {
template <> AngleOf<signed short> AngleOf<signed short>::Radians(float angle) {
if (!isfinite(angle))
return AngleOf<signed short>(0);
@ -84,29 +80,25 @@ AngleOf<signed short> AngleOf<signed short>::Radians(float angle) {
return AngleOf<signed short>(value);
}
template <>
float AngleOf<signed short>::InDegrees() const {
template <> float AngleOf<signed short>::InDegrees() const {
float degrees = this->value / 65536.0f * 360.0f;
return degrees;
}
template <>
float AngleOf<signed short>::InRadians() const {
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) {
template <> AngleOf<signed char> AngleOf<signed char>::Degrees(float angle) {
// map float [-180..180) to integer [-128..127)
signed char value = (signed char)(angle / 360.0F * 256.0F);
return AngleOf<signed char>(value);
}
template <>
AngleOf<signed char> AngleOf<signed char>::Radians(float angle) {
template <> AngleOf<signed char> AngleOf<signed char>::Radians(float angle) {
if (!isfinite(angle))
return AngleOf<signed char>(0);
@ -115,14 +107,12 @@ AngleOf<signed char> AngleOf<signed char>::Radians(float angle) {
return AngleOf<signed char>(value);
}
template <>
float AngleOf<signed char>::InDegrees() const {
template <> float AngleOf<signed char>::InDegrees() const {
float degrees = this->value / 256.0f * 360.0f;
return degrees;
}
template <>
float AngleOf<signed char>::InRadians() const {
template <> float AngleOf<signed char>::InRadians() const {
float radians = this->value / 128.0f * pi;
return radians;
}
@ -136,28 +126,23 @@ float AngleOf<signed char>::InRadians() const {
// 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 {
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 {
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 {
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 {
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 {
template <typename T> bool AngleOf<T>::operator<=(AngleOf<T> a) const {
return this->value <= a.value;
}
@ -178,51 +163,47 @@ AngleOf<T> Passer::LinearAlgebra::AngleOf<T>::Abs(AngleOf<T> a) {
return a;
}
template <typename T>
AngleOf<T> AngleOf<T>::operator-() const {
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> 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> 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> 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> 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) {
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) {
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() {
template <typename T> void AngleOf<T>::Normalize() {
float angleValue = this->InDegrees();
if (!isfinite(angleValue))
return;
@ -234,8 +215,7 @@ void AngleOf<T>::Normalize() {
*this = AngleOf::Degrees(angleValue);
}
template <typename T>
AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
template <typename T> AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
float angleValue = angle.InDegrees();
if (!isfinite(angleValue))
return angle;
@ -254,10 +234,9 @@ AngleOf<T> AngleOf<T>::Clamp(AngleOf<T> angle, AngleOf<T> min, AngleOf<T> max) {
}
template <typename T>
AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
AngleOf<T> toAngle,
AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
float maxDegrees) {
maxDegrees = fmaxf(0, maxDegrees); // filter out negative distances
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));
@ -267,29 +246,23 @@ AngleOf<T> AngleOf<T>::MoveTowards(AngleOf<T> fromAngle,
return fromAngle + d;
}
template <typename T>
float AngleOf<T>::Cos(AngleOf<T> a) {
template <typename T> float AngleOf<T>::Cos(AngleOf<T> a) {
return cosf(a.InRadians());
}
template <typename T>
float AngleOf<T>::Sin(AngleOf<T> a) {
template <typename T> float AngleOf<T>::Sin(AngleOf<T> a) {
return sinf(a.InRadians());
}
template <typename T>
float AngleOf<T>::Tan(AngleOf<T> a) {
template <typename T> float AngleOf<T>::Tan(AngleOf<T> a) {
return tanf(a.InRadians());
}
template <typename T>
AngleOf<T> AngleOf<T>::Acos(float f) {
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) {
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) {
template <typename T> AngleOf<T> AngleOf<T>::Atan(float f) {
return AngleOf<T>::Radians(atanf(f));
}
@ -321,7 +294,7 @@ float AngleOf<T>::CosineRuleSide(float a, float b, AngleOf<T> gamma) {
float b2 = b * b;
float d =
a2 + b2 -
2 * a * b * Cos(gamma); // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
2 * a * b * Cos(gamma); // cosf(gamma * Passer::LinearAlgebra::Deg2Rad);
// Catch edge cases where float inacuracies lead tot nans
if (d < 0)
return 0;

26
Angle.h
View File

@ -13,13 +13,13 @@ static float pi = 3.1415927410125732421875F;
static float Rad2Deg = 360.0f / (pi * 2);
static float Deg2Rad = (pi * 2) / 360.0f;
template <typename T>
class AngleOf {
public:
template <typename T> class AngleOf {
public:
AngleOf<T>();
static AngleOf<T> Degrees(float f);
static AngleOf<T> Radians(float f);
static AngleOf<T> Binary(T x);
// const static AngleOf<T> zero;
// const static AngleOf<T> deg90;
@ -29,6 +29,7 @@ class AngleOf {
float InRadians() const;
inline T GetBinary() const { return this->value; }
inline void SetBinary(T x) { this->value = x; }
bool operator==(const AngleOf<T> a) const;
bool operator>(AngleOf<T> a) const;
@ -40,14 +41,14 @@ class AngleOf {
static AngleOf<T> Abs(AngleOf<T> a);
AngleOf<T> operator-() const;
AngleOf<T> operator-(const AngleOf<T>& a) const;
AngleOf<T> operator+(const AngleOf<T>& a) const;
AngleOf<T> operator+=(const AngleOf<T>& a);
AngleOf<T> operator-(const AngleOf<T> &a) const;
AngleOf<T> operator+(const AngleOf<T> &a) const;
AngleOf<T> operator+=(const AngleOf<T> &a);
friend AngleOf<T> operator*(const AngleOf<T>& a, float f) {
friend AngleOf<T> operator*(const AngleOf<T> &a, float f) {
return AngleOf::Degrees((float)a.InDegrees() * f);
}
friend AngleOf<T> operator*(float f, const AngleOf<T>& a) {
friend AngleOf<T> operator*(float f, const AngleOf<T> &a) {
return AngleOf::Degrees((float)f * a.InDegrees());
}
@ -58,8 +59,7 @@ class AngleOf {
// AngleOf<T> r = Normalize(b.InDegrees() - a.InDegrees());
// return r;
// };
static AngleOf<T> MoveTowards(AngleOf<T> fromAngle,
AngleOf<T> toAngle,
static AngleOf<T> MoveTowards(AngleOf<T> fromAngle, AngleOf<T> toAngle,
float maxAngle);
static float Cos(AngleOf<T> a);
@ -76,7 +76,7 @@ class AngleOf {
static AngleOf<T> SineRuleAngle(float a, AngleOf<T> beta, float c);
private:
private:
T value;
AngleOf<T>(T value);
@ -91,8 +91,8 @@ using AngleSingle = AngleOf<float>;
using Angle16 = AngleOf<signed short>;
using Angle8 = AngleOf<signed char>;
} // namespace LinearAlgebra
} // namespace Passer
} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;
#endif