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