RoboidControl-cpp/test/SphericalSingle_test.cc
2024-12-26 12:34:26 +01:00

213 lines
7.7 KiB
C++

#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>
#include "Spherical.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(SphericalSingle, FromVector3) {
Vector3 v = Vector3(0, 0, 1);
SphericalSingle s = SphericalSingle ::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 0 1";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 0 0 1";
v = Vector3(0, 1, 0);
s = SphericalSingle ::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F) << "s.hor 0 1 0";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 90.0F) << "s.vert 0 1 0";
v = Vector3(1, 0, 0);
s = SphericalSingle ::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 90.0F) << "s.hor 1 0 0";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F) << "s.vert 1 0 0";
}
TEST(SphericalSingle, FromPolar) {
Polar p = Polar(1, Angle::Degrees(0));
SphericalSingle s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
<< "s.hor Polar(1 0)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(1 0)";
p = Polar(1, Angle::Degrees(45));
s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 45.0F)
<< "s.hor Polar(1 45)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(1 45)";
p = Polar(1, Angle::Degrees(-45));
s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -45.0F)
<< "s.hor Polar(1 -45)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(1 -45)";
p = Polar(0, Angle::Degrees(0));
s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), 0.0F)
<< "s.hor Polar(0 0)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(0 0)";
p = Polar(-1, Angle::Degrees(0));
s = SphericalSingle ::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)";
EXPECT_FLOAT_EQ(s.direction.horizontal.InDegrees(), -180.0F)
<< "s.hor Polar(-1 0)";
EXPECT_FLOAT_EQ(s.direction.vertical.InDegrees(), 0.0F)
<< "s.vert Polar(-1 0)";
}
TEST(SphericalSingle, Incident1) {
Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
SphericalSingle s = SphericalSingle ::FromVector3(v);
SphericalSingle sr =
SphericalSingle(2.49F, Angle::Degrees(98.18f), Angle::Degrees(24.4F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-02);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-02);
Vector3 r = Vector3(sr);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0";
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-02) << "toVector3.y 1 0 0";
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0";
}
TEST(SphericalSingle, Incident2) {
Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
SphericalSingle s = SphericalSingle ::FromVector3(v);
SphericalSingle sr = SphericalSingle(1.4142135623F, Angle::Degrees(45.0f),
Angle::Degrees(0.0F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-05);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-05);
Vector3 r = Vector3(sr);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
v = Vector3(0.0f, 1.0f, 1.0f);
s = SphericalSingle ::FromVector3(v);
sr = SphericalSingle(1.4142135623F, Angle::Degrees(0.0f),
Angle::Degrees(45.0F));
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.direction.horizontal.InDegrees(),
sr.direction.horizontal.InDegrees(), 1.0e-05);
EXPECT_NEAR(s.direction.vertical.InDegrees(),
sr.direction.vertical.InDegrees(), 1.0e-05);
r = Vector3(sr);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
v = Vector3(1.0f, 1.0f, 1.0f);
s = SphericalSingle ::FromVector3(v);
r = Vector3(s);
EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
EXPECT_NEAR(s.direction.horizontal.InDegrees(), 45.0F, 1.0e-02);
EXPECT_NEAR(s.direction.vertical.InDegrees(), 35.26F, 1.0e-02);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
// s = SphericalSingle(10, 45, 45);
// r = s.ToVector3();
// EXPECT_NEAR(r.x, 5, 1.0e-06);
// EXPECT_NEAR(r.y, 7.07, 1.0e-06);
// EXPECT_NEAR(r.z, 5, 1.0e-06);
}
TEST(SphericalSingle, Addition) {
SphericalSingle v1 =
SphericalSingle(1, Angle::Degrees(45), Angle::Degrees(0));
SphericalSingle v2 = SphericalSingle ::zero;
SphericalSingle r = SphericalSingle ::zero;
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
r = v1;
r += v2;
EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0 0)";
v2 = SphericalSingle(1, Angle::Degrees(-45), Angle::Degrees(0));
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 0) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 0) << "Addition(1 -45 0)";
v2 = SphericalSingle(1, Angle::Degrees(0), Angle::Degrees(90));
r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)";
EXPECT_FLOAT_EQ(r.direction.horizontal.InDegrees(), 45) << "Addition(1 0 90)";
EXPECT_FLOAT_EQ(r.direction.vertical.InDegrees(), 45) << "Addition(1 0 90)";
}
TEST(SphericalSingle, AdditionPerformance) {
const int numIterations = 1000000; // Number of additions to test
std::vector<SphericalSingle> sphericalObjects;
// Populate the vector with random SphericalOf objects
for (int i = 0; i < numIterations; ++i) {
float distance = (float)(rand() % 100);
float horizontal = (float)(rand() % 180);
float vertical = (float)(rand() % 360);
SphericalSingle s = SphericalSingle::Deg(distance, horizontal, vertical);
sphericalObjects.push_back(s);
}
// Measure the time to perform multiple additions
auto start = std::chrono::high_resolution_clock::now();
SphericalSingle result = SphericalSingle::zero; // Start with a
// zero-initialized object
for (int i = 0; i < numIterations - 1; ++i) {
result = result + sphericalObjects[i]; // Add objects
// together
}
auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> duration = end - start;
std::cout << "Time to perform " << numIterations - 1
<< " additions: " << duration.count() << " seconds." << std::endl;
// Assert that the time taken is less than
// 1 second (or any other performance
// requirement)
ASSERT_LE(duration.count(), 1.0) << "Performance test failed: "
"Additions took longer than 1 "
"second.";
}
#endif