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

Fix int/float issues

Browse Source
This commit is contained in:
Pascal Serrarens 2024-08-02 11:55:27 +02:00
parent 86b7c21dca
commit 48828e20b3
11 changed files with 154 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Angle.cpp
View File

@ -73,6 +73,9 @@ float Angle::SineRuleAngle(float a, float beta, float b) {
*/
//----------------------
template <>
AngleOf<float>::AngleOf(int angle) : value(angle) {}
template <>
AngleOf<float>::AngleOf(float angle) : value(angle) {}
@ -127,8 +130,8 @@ AngleOf<float> AngleOf<float>::MoveTowards(AngleOf<float> fromAngle,
AngleOf<float> toAngle,
AngleOf<float> maxAngle) {
float d = toAngle - fromAngle;
float sign = signbit(d) ? -1 : 1;
d = sign * Float::Clamp(fabs(d), 0, maxAngle);
int sign = signbit(d) ? -1 : 1;
d = sign * Float::Clamp(fabsf(d), 0, maxAngle);
return fromAngle + d;
}
@ -141,7 +144,7 @@ AngleOf<float> AngleOf<float>::CosineRuleSide(float a,
float d = a2 + b2 - 2 * a * b * cos(gamma * AngleOf<float>::Deg2Rad);
// Catch edge cases where float inacuracies lead tot nans
if (d < 0)
return 0;
return 0.0f;
float c = sqrtf(d);
return c;
@ -155,9 +158,9 @@ AngleOf<float> AngleOf<float>::CosineRuleAngle(float a, float b, float c) {
float d = (a2 + b2 - c2) / (2 * a * b);
// Catch edge cases where float inacuracies lead tot nans
if (d >= 1)
return 0;
return 0.0f;
if (d <= -1)
return 180;
return 180.0f;
float gamma = acos(d) * Angle::Rad2Deg;
return gamma;

5
Angle.h
View File

@ -12,13 +12,10 @@ template <typename T>
class AngleOf {
public:
AngleOf() {};
AngleOf(int f);
AngleOf(float f);
// AngleOf(T v) : value(v) {}
// operator T() const; // { return value; }
operator float() const;
static AngleOf<T> Degrees(float f);
inline T GetBinary() const { return this->value; }
static AngleOf<T> Rad2Deg;

17
Angle16.cpp
View File

@ -3,8 +3,15 @@
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.
#include <math.h>
#include <stdlib.h>
#include "Angle.h"
template <>
AngleOf<signed short>::AngleOf(int angle) {
signed long long_angle = (signed short)angle * 65535;
this->value = (signed short)(long_angle / 360);
}
template <>
AngleOf<signed short>::AngleOf(float angle) {
if (!isfinite(angle)) {
@ -30,10 +37,10 @@ AngleOf<signed short> AngleOf<signed short>::CosineRuleSide(
AngleOf<signed short> gamma) {
float a2 = a * a;
float b2 = b * b;
float d = a2 + b2 - 2 * a * b * cos(gamma * AngleOf<float>::Deg2Rad);
float d = a2 + b2 - 2 * a * b * cosf(gamma * AngleOf<float>::Deg2Rad);
// Catch edge cases where float inacuracies lead tot nans
if (d < 0)
return 0;
return 0.0f;
float c = sqrtf(d);
return c;
@ -50,10 +57,10 @@ AngleOf<signed short> AngleOf<signed short>::CosineRuleAngle(float a,
float d = (a2 + b2 - c2) / (2 * a * b);
// Catch edge cases where float inacuracies lead tot nans
if (d >= 1)
return 0;
return 0.0f;
if (d <= -1)
return 180;
return 180.0f;
float gamma = acos(d) * Angle::Rad2Deg;
float gamma = acosf(d) * Angle::Rad2Deg;
return gamma;
}

16
Angle16.h
View File

@ -1,6 +1,7 @@
// #include "AngleUsing.h"
#include <math.h>
#include <stdlib.h>
#include "Angle.h"
namespace Passer {
@ -8,10 +9,17 @@ namespace LinearAlgebra {
typedef AngleOf<signed short> Angle16;
template <>
inline static Angle16 Angle16::Degrees(float angle) {
return (angle / 360.0F * 65536.0F);
}
// template <>
// inline static Angle16 Angle16::Degrees(short angle) {
// long long_angle = angle * 65535;
// long_angle = div(long_angle, (long)360);
// return Angle16(long_angle); //(long_angle / 360);
// }
// template <>
// inline static Angle16 Angle16::Degrees(float angle) {
// return (angle / 360.0F * 65536.0F);
// }
// template <> Angle16::AngleOf(float angle) {
// if (!isfinite(angle)) {
// value = 0;

6
Angle8.cpp
View File

@ -5,6 +5,12 @@
#include <math.h>
#include "Angle.h"
template <>
AngleOf<signed char>::AngleOf(int angle) {
signed short short_angle = (signed char)angle * 256;
this->value = (signed char)(short_angle / 360);
}
template <>
AngleOf<signed char>::AngleOf(float angle) {
if (!isfinite(angle)) {

8
Angle8.h
View File

@ -8,10 +8,10 @@ namespace LinearAlgebra {
typedef AngleOf<signed char> Angle8;
template <>
inline static Angle8 Angle8::Degrees(float angle) {
return (angle / 360.0F * 256.0F);
}
// template <>
// inline static Angle8 Angle8::Degrees(float angle) {
// return (angle / 360.0F * 256.0F);
// }
// template <> Angle8::AngleOf(float angle) {
// if (!isfinite(angle)) {

16
Axis.cpp
View File

@ -10,8 +10,8 @@
#include <math.h>
Axis::Axis() {
horizontalAngle = 0;
verticalAngle = 0;
horizontalAngle = 0.0f;
verticalAngle = 0.0f;
}
Axis::Axis(Angle horizontal, Angle vertical) {
@ -24,12 +24,12 @@ Axis::Axis(Vector3 v) {
this->verticalAngle = 90 - acosf(v.Up()) * Angle::Rad2Deg;
}
const Axis Axis::forward = Axis(0, 0);
const Axis Axis::back = Axis(180, 0);
const Axis Axis::up = Axis(0, 90);
const Axis Axis::down = Axis(0, -90);
const Axis Axis::left = Axis(-90, 0);
const Axis Axis::right = Axis(90, 0);
const Axis Axis::forward = Axis(0.0f, 0.0f);
const Axis Axis::back = Axis(180.0f, 0.0f);
const Axis Axis::up = Axis(0.0f, 90.0f);
const Axis Axis::down = Axis(0.0f, -90.0f);
const Axis Axis::left = Axis(-90.0f, 0.0f);
const Axis Axis::right = Axis(90.0f, 0.0f);
Vector3 Axis::ToVector3() {
Vector3 v =

1
CMakeLists.txt
View File

@ -14,6 +14,7 @@ else()
include(FetchContent)
FetchContent_Declare(
googletest
DOWNLOAD_EXTRACT_TIMESTAMP
URL https://github.com/google/googletest/archive/refs/heads/main.zip
)

2
Spherical.cpp
View File

@ -190,7 +190,7 @@ Angle Spherical::AngleBetween(const Spherical& v1, const Spherical& v2) {
v1.distance * v2.distance; // sqrtf(v1.distance * v1.distance *
// v2.distance * v2.distance);
if (denominator < epsilon)
return 0;
return 0.0f;
Vector3 v1_3 = Vector3(v1);
Vector3 v2_3 = Vector3(v2);

200
test/Angle_test.cc
View File

@ -9,159 +9,159 @@
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle, Normalize) {
float r = 0;
float r = 0;
r = Angle::Normalize(90);
EXPECT_FLOAT_EQ(r, 90) << "Normalize 90";
r = Angle::Normalize(90.0f);
EXPECT_FLOAT_EQ(r, 90) << "Normalize 90";
r = Angle::Normalize(-90);
EXPECT_FLOAT_EQ(r, -90) << "Normalize -90";
r = Angle::Normalize(-90);
EXPECT_FLOAT_EQ(r, -90) << "Normalize -90";
r = Angle::Normalize(270);
EXPECT_FLOAT_EQ(r, -90) << "Normalize 270";
r = Angle::Normalize(270);
EXPECT_FLOAT_EQ(r, -90) << "Normalize 270";
r = Angle::Normalize(270+360);
EXPECT_FLOAT_EQ(r, -90) << "Normalize 270+360";
r = Angle::Normalize(270 + 360);
EXPECT_FLOAT_EQ(r, -90) << "Normalize 270+360";
r = Angle::Normalize(-270);
EXPECT_FLOAT_EQ(r, 90) << "Normalize -270";
r = Angle::Normalize(-270);
EXPECT_FLOAT_EQ(r, 90) << "Normalize -270";
r = Angle::Normalize(-270 - 360);
EXPECT_FLOAT_EQ(r, 90) << "Normalize -270-360";
r = Angle::Normalize(-270 - 360);
EXPECT_FLOAT_EQ(r, 90) << "Normalize -270-360";
r = Angle::Normalize(0);
EXPECT_FLOAT_EQ(r, 0) << "Normalize 0";
r = Angle::Normalize(0);
EXPECT_FLOAT_EQ(r, 0) << "Normalize 0";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::Normalize(FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r, FLOAT_INFINITY) << "Normalize INFINITY";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::Normalize(FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r, FLOAT_INFINITY) << "Normalize INFINITY";
r = Angle::Normalize(-FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r, -FLOAT_INFINITY) << "Normalize INFINITY";
}
r = Angle::Normalize(-FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r, -FLOAT_INFINITY) << "Normalize INFINITY";
}
}
TEST(Angle, Clamp) {
float r = 0;
float r = 0;
r = Angle::Clamp(1, 0, 2);
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 2";
r = Angle::Clamp(1, 0, 2);
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 2";
r = Angle::Clamp(-1, 0, 2);
EXPECT_FLOAT_EQ(r, 0) << "Clamp -1 0 2";
r = Angle::Clamp(-1, 0, 2);
EXPECT_FLOAT_EQ(r, 0) << "Clamp -1 0 2";
r = Angle::Clamp(3, 0, 2);
EXPECT_FLOAT_EQ(r, 2) << "Clamp 3 0 2";
r = Angle::Clamp(3, 0, 2);
EXPECT_FLOAT_EQ(r, 2) << "Clamp 3 0 2";
r = Angle::Clamp(1, 0, 0);
EXPECT_FLOAT_EQ(r, 0) << "Clamp 1 0 0";
r = Angle::Clamp(1, 0, 0);
EXPECT_FLOAT_EQ(r, 0) << "Clamp 1 0 0";
r = Angle::Clamp(0, 0, 0);
EXPECT_FLOAT_EQ(r, 0) << "Clamp 0 0 0";
r = Angle::Clamp(0, 0, 0);
EXPECT_FLOAT_EQ(r, 0) << "Clamp 0 0 0";
r = Angle::Clamp(0, 1, -1);
EXPECT_FLOAT_EQ(r, 1) << "Clamp 0 1 -1";
r = Angle::Clamp(0, 1, -1);
EXPECT_FLOAT_EQ(r, 1) << "Clamp 0 1 -1";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::Clamp(1, 0, FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 INFINITY";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::Clamp(1, 0, FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 0 INFINITY";
r = Angle::Clamp(1, -FLOAT_INFINITY, 1);
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 -INFINITY 1";
}
r = Angle::Clamp(1, -FLOAT_INFINITY, 1);
EXPECT_FLOAT_EQ(r, 1) << "Clamp 1 -INFINITY 1";
}
}
TEST(Angle, Difference) {
float r = 0;
float r = 0;
r = Angle::Difference(0, 90);
EXPECT_FLOAT_EQ(r, 90) << "Difference 0 90";
r = Angle::Difference(0, 90);
EXPECT_FLOAT_EQ(r, 90) << "Difference 0 90";
r = Angle::Difference(0, -90);
EXPECT_FLOAT_EQ(r, -90) << "Difference 0 -90";
r = Angle::Difference(0, -90);
EXPECT_FLOAT_EQ(r, -90) << "Difference 0 -90";
r = Angle::Difference(0, 270);
EXPECT_FLOAT_EQ(r, -90) << "Difference 0 270";
r = Angle::Difference(0, 270);
EXPECT_FLOAT_EQ(r, -90) << "Difference 0 270";
r = Angle::Difference(0, -270);
EXPECT_FLOAT_EQ(r, 90) << "Difference 0 -270";
r = Angle::Difference(0, -270);
EXPECT_FLOAT_EQ(r, 90) << "Difference 0 -270";
r = Angle::Difference(90, 0);
EXPECT_FLOAT_EQ(r, -90) << "Difference 90 0";
r = Angle::Difference(90, 0);
EXPECT_FLOAT_EQ(r, -90) << "Difference 90 0";
r = Angle::Difference(-90, 0);
EXPECT_FLOAT_EQ(r, 90) << "Difference -90 0";
r = Angle::Difference(-90, 0);
EXPECT_FLOAT_EQ(r, 90) << "Difference -90 0";
r = Angle::Difference(0, 0);
EXPECT_FLOAT_EQ(r, 0) << "Difference 0 0";
r = Angle::Difference(0, 0);
EXPECT_FLOAT_EQ(r, 0) << "Difference 0 0";
r = Angle::Difference(90, 90);
EXPECT_FLOAT_EQ(r, 0) << "Difference 90 90";
r = Angle::Difference(90, 90);
EXPECT_FLOAT_EQ(r, 0) << "Difference 90 90";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::Difference(0, INFINITY);
EXPECT_FLOAT_EQ(r, INFINITY) << "Difference 0 INFINITY";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::Difference(0, INFINITY);
EXPECT_FLOAT_EQ(r, INFINITY) << "Difference 0 INFINITY";
r = Angle::Difference(0, -INFINITY);
EXPECT_FLOAT_EQ(r, -INFINITY) << "Difference 0 -INFINITY";
r = Angle::Difference(0, -INFINITY);
EXPECT_FLOAT_EQ(r, -INFINITY) << "Difference 0 -INFINITY";
r = Angle::Difference(-INFINITY, INFINITY);
EXPECT_FLOAT_EQ(r, INFINITY) << "Difference -INFINITY INFINITY";
}
r = Angle::Difference(-INFINITY, INFINITY);
EXPECT_FLOAT_EQ(r, INFINITY) << "Difference -INFINITY INFINITY";
}
}
TEST(Angle, MoveTowards) {
float r = 0;
float r = 0;
r = Angle::MoveTowards(0, 90, 30);
EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 90 30";
r = Angle::MoveTowards(0, 90, 30);
EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 90 30";
r = Angle::MoveTowards(0, 90, 90);
EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 90";
r = Angle::MoveTowards(0, 90, 90);
EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 90";
r = Angle::MoveTowards(0, 90, 180);
EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 180";
r = Angle::MoveTowards(0, 90, 180);
EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 180";
r = Angle::MoveTowards(0, 90, 270);
EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 270";
r = Angle::MoveTowards(0, 90, 270);
EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 270";
r = Angle::MoveTowards(0, 90, -30);
EXPECT_FLOAT_EQ(r, -30) << "MoveTowards 0 90 -30";
r = Angle::MoveTowards(0, 90, -30);
EXPECT_FLOAT_EQ(r, -30) << "MoveTowards 0 90 -30";
r = Angle::MoveTowards(0, -90, -30);
EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 -90 -30";
r = Angle::MoveTowards(0, -90, -30);
EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 -90 -30";
r = Angle::MoveTowards(0, -90, -90);
EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 -90 -90";
r = Angle::MoveTowards(0, -90, -90);
EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 -90 -90";
r = Angle::MoveTowards(0, -90, -180);
EXPECT_FLOAT_EQ(r, 180) << "MoveTowards 0 -90 -180";
r = Angle::MoveTowards(0, -90, -180);
EXPECT_FLOAT_EQ(r, 180) << "MoveTowards 0 -90 -180";
r = Angle::MoveTowards(0, -90, -270);
EXPECT_FLOAT_EQ(r, 270) << "MoveTowards 0 -90 -270";
r = Angle::MoveTowards(0, -90, -270);
EXPECT_FLOAT_EQ(r, 270) << "MoveTowards 0 -90 -270";
r = Angle::MoveTowards(0, 90, 0);
EXPECT_FLOAT_EQ(r, 0) << "MoveTowards 0 90 0";
r = Angle::MoveTowards(0, 90, 0);
EXPECT_FLOAT_EQ(r, 0) << "MoveTowards 0 90 0";
r = Angle::MoveTowards(0, 0, 0);
EXPECT_FLOAT_EQ(r, 0) << "MoveTowards 0 0 0";
r = Angle::MoveTowards(0, 0, 0);
EXPECT_FLOAT_EQ(r, 0) << "MoveTowards 0 0 0";
r = Angle::MoveTowards(0, 0, 30);
EXPECT_FLOAT_EQ(r, 0) << "MoveTowards 0 0 30";
r = Angle::MoveTowards(0, 0, 30);
EXPECT_FLOAT_EQ(r, 0) << "MoveTowards 0 0 30";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::MoveTowards(0, 90, FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 FLOAT_INFINITY";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::MoveTowards(0, 90, FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r, 90) << "MoveTowards 0 90 FLOAT_INFINITY";
r = Angle::MoveTowards(0, FLOAT_INFINITY, 30);
EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 FLOAT_INFINITY 30";
r = Angle::MoveTowards(0, FLOAT_INFINITY, 30);
EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 FLOAT_INFINITY 30";
r = Angle::MoveTowards(0, -90, -FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r, FLOAT_INFINITY) << "MoveTowards 0 -90 -FLOAT_INFINITY";
r = Angle::MoveTowards(0, -90, -FLOAT_INFINITY);
EXPECT_FLOAT_EQ(r, FLOAT_INFINITY) << "MoveTowards 0 -90 -FLOAT_INFINITY";
r = Angle::MoveTowards(0, -FLOAT_INFINITY, -30);
EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 -FLOAT_INFINITY -30";
}
r = Angle::MoveTowards(0, -FLOAT_INFINITY, -30);
EXPECT_FLOAT_EQ(r, 30) << "MoveTowards 0 -FLOAT_INFINITY -30";
}
}
#endif

2
test/DiscreteAngle_test.cc
View File

@ -36,7 +36,7 @@ TEST(Angle8, Negate) {
}
TEST(Angle8, Add) {
Angle8 a = Angle8::Degrees(0.0F);
Angle8 a = Angle8(0);
Angle8 b = Angle8(0.0F);
Angle8 r = a + b;
EXPECT_FLOAT_EQ((float)r, 0);