RoboidControl-cpp/test/Angle_test.cc
2024-09-24 10:29:21 +02:00

222 lines
7.6 KiB
C++

#if GTEST
#include <gtest/gtest.h>
#include <math.h>
#include <limits>
#include "Angle.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Angle16, Compare) {
Angle16 a = Angle16::Degrees(45);
bool r = false;
r = a > Angle16::Degrees(0);
EXPECT_TRUE(r) << "45 > 0";
r = a > Angle16::Degrees(90);
EXPECT_FALSE(r) << "45 > 90";
r = a > Angle16::Degrees(-90);
EXPECT_TRUE(r) << "45 > -90";
}
TEST(AngleSingle, Compare) {
AngleSingle a = AngleSingle::Degrees(45);
bool r = false;
r = a > AngleSingle::Degrees(0);
EXPECT_TRUE(r) << "45 > 0";
r = a > AngleSingle::Degrees(90);
EXPECT_FALSE(r) << "45 > 90";
r = a > AngleSingle::Degrees(-90);
EXPECT_TRUE(r) << "45 > -90";
}
TEST(Angle, Normalize) {
Angle r = AngleSingle();
r = Angle::Normalize(AngleSingle::Degrees(90.0f));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize 90";
r = Angle::Normalize(AngleSingle::Degrees(-90));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize -90";
r = Angle::Normalize(AngleSingle::Degrees(270));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270";
r = Angle::Normalize(AngleSingle::Degrees(270 + 360));
EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Normalize 270+360";
r = Angle::Normalize(AngleSingle::Degrees(-270));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270";
r = Angle::Normalize(AngleSingle::Degrees(-270 - 360));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Normalize -270-360";
r = Angle::Normalize(AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Normalize 0";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::Normalize(AngleSingle::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY) << "Normalize INFINITY";
r = Angle::Normalize(AngleSingle::Degrees(-FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), -FLOAT_INFINITY) << "Normalize INFINITY";
}
}
TEST(Angle, Clamp) {
Angle r = AngleSingle();
r = Angle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 2";
r = Angle::Clamp(AngleSingle::Degrees(-1), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp -1 0 2";
r = Angle::Clamp(AngleSingle::Degrees(3), AngleSingle::Degrees(0),
AngleSingle::Degrees(2));
EXPECT_FLOAT_EQ(r.InDegrees(), 2) << "Clamp 3 0 2";
r = Angle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 1 0 0";
r = Angle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(0),
AngleSingle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Clamp 0 0 0";
r = Angle::Clamp(AngleSingle::Degrees(0), AngleSingle::Degrees(1),
AngleSingle::Degrees(-1));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 0 1 -1";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::Clamp(AngleSingle::Degrees(1), AngleSingle::Degrees(0),
AngleSingle::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 0 INFINITY";
r = Angle::Clamp(AngleSingle::Degrees(1),
AngleSingle::Degrees(-FLOAT_INFINITY),
AngleSingle::Degrees(1));
EXPECT_FLOAT_EQ(r.InDegrees(), 1) << "Clamp 1 -INFINITY 1";
}
}
// TEST(Angle, Difference) {
// Angle r = 0;
// r = Angle::Difference(0, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 90";
// r = Angle::Difference(0, -90);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 -90";
// r = Angle::Difference(0, 270);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 0 270";
// r = Angle::Difference(0, -270);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference 0 -270";
// r = Angle::Difference(90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), -90) << "Difference 90 0";
// r = Angle::Difference(-90, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "Difference -90 0";
// r = Angle::Difference(0, 0);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 0 0";
// r = Angle::Difference(90, 90);
// EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "Difference 90 90";
// if (std::numeric_limits<float>::is_iec559) {
// r = Angle::Difference(0, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference 0 INFINITY";
// r = Angle::Difference(0, -INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), -INFINITY) << "Difference 0 -INFINITY";
// r = Angle::Difference(-INFINITY, INFINITY);
// EXPECT_FLOAT_EQ(r.InDegrees(), INFINITY) << "Difference -INFINITY
// INFINITY";
// }
// }
TEST(Angle, MoveTowards) {
Angle r = Angle();
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(30));
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 90 30";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(90));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 90";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(180));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 180";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(270));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 270";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(-30));
EXPECT_FLOAT_EQ(r.InDegrees(), -30) << "MoveTowards 0 90 -30";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
Angle::Degrees(-30));
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -90 -30";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
Angle::Degrees(-90));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 -90 -90";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
Angle::Degrees(-180));
EXPECT_FLOAT_EQ(r.InDegrees(), 180) << "MoveTowards 0 -90 -180";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
Angle::Degrees(-270));
EXPECT_FLOAT_EQ(r.InDegrees(), 270) << "MoveTowards 0 -90 -270";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 90 0";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(0),
Angle::Degrees(0));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 0";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(0),
Angle::Degrees(30));
EXPECT_FLOAT_EQ(r.InDegrees(), 0) << "MoveTowards 0 0 30";
if (std::numeric_limits<float>::is_iec559) {
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(90),
Angle::Degrees(FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), 90) << "MoveTowards 0 90 FLOAT_INFINITY";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(FLOAT_INFINITY),
Angle::Degrees(30));
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 FLOAT_INFINITY 30";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-90),
Angle::Degrees(-FLOAT_INFINITY));
EXPECT_FLOAT_EQ(r.InDegrees(), FLOAT_INFINITY)
<< "MoveTowards 0 -90 -FLOAT_INFINITY";
r = Angle::MoveTowards(Angle::Degrees(0), Angle::Degrees(-FLOAT_INFINITY),
Angle::Degrees(-30));
EXPECT_FLOAT_EQ(r.InDegrees(), 30) << "MoveTowards 0 -FLOAT_INFINITY -30";
}
}
#endif