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

First Discrete Angle functions

Browse Source
This commit is contained in:
Pascal Serrarens 2024-03-15 10:46:30 +01:00
parent 87d2c11ab6
commit e62bacc6b4
8 changed files with 233 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
Angle16.h Normal file
View File

@ -0,0 +1,21 @@
#include "AngleUsing.h"
#include "Angle.h"
#include <math.h>
typedef AngleUsing<signed short> Angle16;
Angle16::AngleUsing(float angle) {
if (!isfinite(angle)) {
value = 0;
return;
}
// map float [-180..180) to integer [-32768..32767]
this->value = (signed short)((angle / 360.0F) * 65536.0F);
}
float Angle16::ToFloat() const {
float f = ((this->value * 180) / 32768.0F);
return f;
}

22
Angle8.h Normal file
View File

@ -0,0 +1,22 @@
#include "AngleUsing.h"
#include "Angle.h"
#include <math.h>
typedef AngleUsing<signed char> Angle8;
Angle8::AngleUsing(float angle) {
if (!isfinite(angle)) {
value = 0;
return;
}
// map float [-180..180) to integer [-128..127]
float f = angle / 360.0F;
this->value = (signed char)(f * 256.0F);
}
float Angle8::ToFloat() const {
float f = (this->value * 180) / 128.0F;
return f;
}

83
AngleUsing.cpp Normal file
View File

@ -0,0 +1,83 @@
// 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 "DiscreteAngle.h"
#include "Angle.h"
#include <math.h>
// 1-byte angle
// template <> AngleUsing<unsigned char>::AngleUsing(unsigned char angle) {
// if (!isfinite(angle)) {
// value = 0;
// return;
// }
// // normalize to (-180..180]
// angle = Angle::Normalize(angle);
// // map (-180..180] to (0..255], which is equivaluent to 1..255
// // This means that range value 0 is not used
// this->value = (unsigned char)((angle + 180.0F) / 360.0F * 255.0F);
// }
// template <> float AngleUsing<unsigned char>::ToFloat() const {
// float f = ((float)this->value / 255.0F) * 360.0F - 180.0F;
// return f;
// }
// template <> AngleUsing<unsigned char>::AngleUsing(float angle) {
// if (!isfinite(angle)) {
// value = 0;
// return;
// }
// // normalize to (-180..180]
// angle = Angle::Normalize(angle);
// // map (-180..180] to (0..255], which is equivaluent to 1..255
// // This means that range value 0 is not used
// this->value = (unsigned char)((angle + 180.0F) / 360.0F * 255.0F);
// }
// template <> float AngleUsing<unsigned char>::ToFloat() const {
// float f = ((float)this->value / 255.0F) * 360.0F - 180.0F;
// return f;
// }
// 2-byte angle
template <> AngleUsing<unsigned short>::AngleUsing(unsigned short angle) {
if (!isfinite(angle)) {
value = 0;
return;
}
// normalize to (-180..180]
angle = Angle::Normalize(angle);
// map (-180..180] to (0..65535] which is equivalent to 1..65535
// This means that range value 0 is not used
this->value = (unsigned short)((angle + 180.0F) / 360.0F * 65535.0F);
}
template <> AngleUsing<unsigned short>::AngleUsing(float angle) {
if (!isfinite(angle)) {
value = 0;
return;
}
// normalize to (-180..180]
angle = Angle::Normalize(angle);
// map (-180..180] to (0..65535] which is equivalent to 1..65535
// This means that range value 0 is not used
this->value = (unsigned short)((angle + 180.0F) / 360.0F * 65535.0F);
}
template <> float AngleUsing<unsigned short>::ToFloat() const {
float f = ((float)this->value / 65535.0F) * 360.0F - 180.0F;
return f;
}

42
AngleUsing.h Normal file
View File

@ -0,0 +1,42 @@
#ifndef DISCRETEANGLE_H
#define DISCRETEANGLE_H
#include "Angle.h"
#include "Range.h"
// A fixed angle between (-180..180]
template <typename T> class AngleUsing {
public:
AngleUsing(T sourceValue) { this->value = sourceValue; }
AngleUsing(float f);
float ToFloat() const;
inline T GetValue() const { return this->value; }
AngleUsing<T> operator+(const AngleUsing<T> a) {
AngleUsing<T> r = AngleUsing(this->value + a.value);
return r;
}
inline AngleUsing<T> operator+=(const AngleUsing<T> a) {
return this->value + a.value;
}
inline AngleUsing<T> operator-(const AngleUsing<T> a) {
return this->value - a.value;
}
inline AngleUsing<T> operator-() {
this->value = -this->value;
return *this;
}
inline bool operator==(const AngleUsing<T> a) {
return this->value == a.value;
}
// protected:
T value;
};
#endif

1
CMakeLists.txt
View File

@ -45,6 +45,7 @@ else()
"test/Matrix_test.cc" "test/Matrix_test.cc"
"test/Polar_test.cc" "test/Polar_test.cc"
"test/Spherical_test.cc" "test/Spherical_test.cc"
"test/DiscreteAngle_test.cc"
) )
target_link_libraries( target_link_libraries(
VectorAlgebraTest VectorAlgebraTest

50
DiscreteAngle.cpp
View File

@ -1,50 +0,0 @@
// 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 "DiscreteAngle.h"
#include "Angle.h"
#include <math.h>
// 1-byte angle
template <> AngleUsing<unsigned char>::AngleUsing(unsigned char angle) {
if (!isfinite(angle)) {
value = 0;
return;
}
// normalize to (-180..180]
angle = Angle::Normalize(angle);
// map (-180..180] to (0..255], which is equivaluent to 1..255
// This means that range value 0 is not used
this->value = (unsigned char)((angle + 180.0F) / 360.0F * 255.0F);
}
template <> float AngleUsing<unsigned char>::ToFloat() const {
float f = ((float)this->value / 255.0F) * 360.0F - 180.0F;
return f;
}
// 2-byte angle
template <> AngleUsing<unsigned short>::AngleUsing(unsigned short angle) {
if (!isfinite(angle)) {
value = 0;
return;
}
// normalize to (-180..180]
angle = Angle::Normalize(angle);
// map (-180..180] to (0..65535] which is equivalent to 1..65535
// This means that range value 0 is not used
this->value = (unsigned short)((angle + 180.0F) / 360.0F * 65535.0F);
}
template <> float AngleUsing<unsigned short>::ToFloat() const {
float f = ((float)this->value / 65535.0F) * 360.0F - 180.0F;
return f;
}

35
DiscreteAngle.h
View File

@ -1,35 +0,0 @@
#ifndef DISCRETEANGLE_H
#define DISCRETEANGLE_H
#include "Range.h"
// A fixed angle between (-180..180]
template <typename T> class AngleUsing {
public:
AngleUsing(T sourceValue);
float ToFloat() const;
inline T GetValue() const { return this->value; }
inline AngleUsing<T> operator+(const AngleUsing<T> a) {
return this->value + a.value;
}
inline AngleUsing<T> operator+=(const AngleUsing<T> a &) {
return this->value + a.value;
}
inline AngleUsing<T> operator-(const AngleUsing<T> a) {
return this->value - a.value;
}
inline AngleUsing<T> operator-() { return -this->value; }
inline bool operator==(const AngleUsing<T> a, const AngleUsing<T> b) {
return a.value == b.value;
}
protected:
T value;
};
#endif

64
test/DiscreteAngle_test.cc Normal file
View File

@ -0,0 +1,64 @@
#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "Angle16.h"
#include "Angle8.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle8, Construct) {
float angle = 0.0F;
Angle8 a = Angle8(angle);
float f = a.ToFloat();
EXPECT_FLOAT_EQ(f, angle);
angle = -180.0F;
a = Angle8(angle);
f = a.ToFloat();
EXPECT_FLOAT_EQ(f, angle);
}
TEST(Angle8, Negate) {
float angle = 0;
Angle8 a = Angle8(angle);
a = -a;
float f = a.ToFloat();
EXPECT_FLOAT_EQ(f, angle);
angle = 90.0F;
a = Angle8(angle);
a = -a;
f = a.ToFloat();
EXPECT_FLOAT_EQ(f, -angle);
}
TEST(Angle8, Add) {
Angle8 a = Angle8(0.0F);
Angle8 b = Angle8(0.0F);
Angle8 r = a + b;
EXPECT_FLOAT_EQ(r.ToFloat(), 0);
}
TEST(Angle16, Construct) {
Angle16 a = Angle16(0.0F);
float f = a.ToFloat();
EXPECT_FLOAT_EQ(f, 0);
}
TEST(Angle16, Negate) {
float angle = 0;
Angle16 a = Angle16(angle);
a = -a;
float f = a.ToFloat();
EXPECT_FLOAT_EQ(f, angle);
angle = 90.0F;
a = Angle16(angle);
a = -a;
f = a.ToFloat();
EXPECT_FLOAT_EQ(f, -angle);
}
#endif