RoboidControl-cpp/test/Spherical_test.cc
2024-03-15 17:20:21 +01:00

119 lines
3.5 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(Spherical, FromVector3) {
Vector3 v = Vector3(0, 0, 1);
Spherical s = Spherical(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1";
EXPECT_FLOAT_EQ(s.horizontalAngle, 0.0F) << "s.hor 0 0 1";
EXPECT_FLOAT_EQ(s.verticalAngle, 0.0F) << "s.vert 0 0 1";
v = Vector3(0, 1, 0);
s = Spherical(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0";
EXPECT_FLOAT_EQ(s.horizontalAngle, 0.0F) << "s.hor 0 1 0";
EXPECT_FLOAT_EQ(s.verticalAngle, 90.0F) << "s.vert 0 1 0";
v = Vector3(1, 0, 0);
s = Spherical(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0";
EXPECT_FLOAT_EQ(s.horizontalAngle, 90.0F) << "s.hor 1 0 0";
EXPECT_FLOAT_EQ(s.verticalAngle, 0.0F) << "s.vert 1 0 0";
}
TEST(Spherical, ToVector3) {
Vector3 v = Vector3(0, 0, 1);
Spherical s = Spherical(v);
Vector3 r = s.ToVector3();
EXPECT_FLOAT_EQ(r.x, 0.0F) << "toVector3.x 0 0 1";
EXPECT_NEAR(r.y, 0.0F, 1.0e-06) << "toVector3.y 0 0 1";
EXPECT_FLOAT_EQ(r.z, 1.0F) << "toVector3.z 0 0 1";
v = Vector3(0, 1, 0);
s = Spherical(v);
r = s.ToVector3();
EXPECT_FLOAT_EQ(r.x, 0.0F) << "toVector3.x 0 1 0";
EXPECT_FLOAT_EQ(r.y, 1.0F) << "toVector3.y 0 1 0";
EXPECT_NEAR(r.z, 0.0F, 1.0e-06) << "toVector3.z 0 1 0";
v = Vector3(1, 0, 0);
s = Spherical(v);
r = s.ToVector3();
EXPECT_FLOAT_EQ(r.x, 1.0F) << "toVector3.x 1 0 0";
EXPECT_NEAR(r.y, 0.0F, 1.0e-06) << "toVector3.y 1 0 0";
EXPECT_NEAR(r.z, 0.0F, 1.0e-06) << "toVector3.z 1 0 0";
}
TEST(Spherical, Incident1) {
Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
Spherical s = Spherical(v);
Spherical sr = Spherical(2.49F, 98.18f, 24.4F);
EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
EXPECT_NEAR(s.horizontalAngle, sr.horizontalAngle, 1.0e-02);
EXPECT_NEAR(s.verticalAngle, sr.verticalAngle, 1.0e-02);
Vector3 r = sr.ToVector3();
EXPECT_NEAR(r.x, v.x, 1.0e-02) << "toVector3.x 1 0 0";
EXPECT_NEAR(r.y, v.y, 1.0e-02) << "toVector3.y 1 0 0";
EXPECT_NEAR(r.z, v.z, 1.0e-02) << "toVector3.z 1 0 0";
}
TEST(Spherical, Incident2) {
Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
Spherical s = Spherical(v);
Spherical sr = Spherical(1.4142135623F, 45.0f, 0.0F);
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.horizontalAngle, sr.horizontalAngle, 1.0e-05);
EXPECT_NEAR(s.verticalAngle, sr.verticalAngle, 1.0e-05);
Vector3 r = sr.ToVector3();
EXPECT_NEAR(r.x, v.x, 1.0e-06);
EXPECT_NEAR(r.y, v.y, 1.0e-06);
EXPECT_NEAR(r.z, v.z, 1.0e-06);
v = Vector3(0.0f, 1.0f, 1.0f);
s = Spherical(v);
sr = Spherical(1.4142135623F, 0.0f, 45.0F);
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.horizontalAngle, sr.horizontalAngle, 1.0e-05);
EXPECT_NEAR(s.verticalAngle, sr.verticalAngle, 1.0e-05);
r = sr.ToVector3();
EXPECT_NEAR(r.x, v.x, 1.0e-06);
EXPECT_NEAR(r.y, v.y, 1.0e-06);
EXPECT_NEAR(r.z, v.z, 1.0e-06);
v = Vector3(1.0f, 1.0f, 1.0f);
s = Spherical(v);
r = s.ToVector3();
EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
EXPECT_NEAR(s.horizontalAngle, 45.0F, 1.0e-02);
EXPECT_NEAR(s.verticalAngle, 35.26F, 1.0e-02);
EXPECT_NEAR(r.x, v.x, 1.0e-06);
EXPECT_NEAR(r.y, v.y, 1.0e-06);
EXPECT_NEAR(r.z, v.z, 1.0e-06);
// s = Spherical(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);
}
#endif