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

Fix unit tests

Browse Source
This commit is contained in:
Pascal Serrarens 2024-08-22 14:24:33 +02:00
parent 1da93cac15
commit dedaa31740
5 changed files with 77 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
Angle.cpp
View File

@ -6,10 +6,9 @@
#include <math.h> #include <math.h>
#include "FloatSingle.h" #include "FloatSingle.h"
const float Rad2Deg = 57.29578F;
const float Deg2Rad = 0.0174532924F;
/* /*
const float Angle::Rad2Deg = 57.29578F;
const float Angle::Deg2Rad = 0.0174532924F;
float Angle::Normalize(float angle) { float Angle::Normalize(float angle) {
if (!isfinite(angle)) if (!isfinite(angle))
return angle; return angle;
@ -128,17 +127,17 @@ AngleOf<T> AngleOf<T>::operator+=(const AngleOf<T>& a) {
return *this; return *this;
} }
template <> template <typename T>
AngleOf<float> AngleOf<float>::Normalize(AngleOf<float> angle) { AngleOf<T> AngleOf<T>::Normalize(AngleOf<T> angle) {
float angleValue = angle.ToFloat(); float angleValue = angle.ToFloat();
if (!isfinite(angleValue)) if (!isfinite(angleValue))
return angleValue; return angle;
while (angleValue <= -180) while (angleValue <= -180)
angleValue += 360; angleValue += 360;
while (angleValue > 180) while (angleValue > 180)
angleValue -= 360; angleValue -= 360;
return angleValue; return AngleOf(angleValue);
} }
template <> template <>
@ -194,8 +193,7 @@ AngleOf<float> AngleOf<float>::CosineRuleAngle(float a, float b, float c) {
if (d <= -1) if (d <= -1)
return 180.0f; return 180.0f;
float rad2deg = Passer::LinearAlgebra::Rad2Deg; float gamma = acosf(d) * Rad2Deg;
float gamma = acosf(d) * rad2deg;
return gamma; return gamma;
} }
@ -203,9 +201,10 @@ template <>
AngleOf<float> AngleOf<float>::SineRuleAngle(float a, AngleOf<float> AngleOf<float>::SineRuleAngle(float a,
AngleOf<float> beta, AngleOf<float> beta,
float b) { float b) {
float deg2rad = Passer::LinearAlgebra::Deg2Rad; float deg2rad = Deg2Rad;
float alpha = asinf(a * sinf(beta.ToFloat() * deg2rad) / b); float alpha = asinf(a * sinf(beta.ToFloat() * deg2rad) / b);
return alpha; return alpha;
} }
template class AngleOf<float>; template class AngleOf<float>;
template class AngleOf<signed short>; template class AngleOf<signed short>;

3
Angle.h
View File

@ -25,9 +25,6 @@ class AngleOf {
inline T GetBinary() const { return this->value; } inline T GetBinary() const { return this->value; }
static AngleOf<T> Rad2Deg;
static AngleOf<T> Deg2Rad;
static AngleOf<T> pi; static AngleOf<T> pi;
bool operator==(AngleOf<T> a); bool operator==(AngleOf<T> a);

9
Spherical.cpp
View File

@ -46,6 +46,15 @@ SphericalOf<T>::SphericalOf(float distance,
// this->vertical = vertical; // this->vertical = vertical;
// } // }
template <>
SphericalOf<float> SphericalOf<float>::FromPolar(Polar polar) {
AngleOf<float> horizontal = polar.angle;
AngleOf<float> vertical = AngleOf<float>(0);
SphericalOf<float> r =
SphericalOf<float>(polar.distance, horizontal, vertical);
return r;
}
template <typename T> template <typename T>
SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) { SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
float distance = v.magnitude(); float distance = v.magnitude();

2
Spherical.h
View File

@ -29,6 +29,8 @@ class SphericalOf {
SphericalOf<T>(); SphericalOf<T>();
SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical); SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
static SphericalOf<T> FromPolar(Polar v);
static SphericalOf<T> FromVector3(Vector3 v); static SphericalOf<T> FromVector3(Vector3 v);
Vector3 ToVector3() const; Vector3 ToVector3() const;

121
test/Spherical16_test.cc
View File

@ -3,82 +3,80 @@
#include <math.h> #include <math.h>
#include <limits> #include <limits>
#include "Spherical16.h" #include "Spherical.h"
#define FLOAT_INFINITY std::numeric_limits<float>::infinity() #define FLOAT_INFINITY std::numeric_limits<float>::infinity()
TEST(Spherical16, FromVector3) { TEST(Spherical16, FromVector3) {
Vector3 v = Vector3(0, 0, 1); Vector3 v = Vector3(0, 0, 1);
Spherical16 s = Spherical16(v); Spherical16 s = Spherical16::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1"; EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 0 1";
EXPECT_FLOAT_EQ((float)s.horizontalAngle.ToFloat(), 0.0F) << "s.hor 0 0 1"; EXPECT_FLOAT_EQ((float)s.horizontal.ToFloat(), 0.0F) << "s.hor 0 0 1";
EXPECT_FLOAT_EQ((float)s.verticalAngle.ToFloat(), 0.0F) << "s.vert 0 0 1"; EXPECT_FLOAT_EQ((float)s.vertical.ToFloat(), 0.0F) << "s.vert 0 0 1";
v = Vector3(0, 1, 0); v = Vector3(0, 1, 0);
s = Spherical16(v); s = Spherical16::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0"; EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 0 1 0";
EXPECT_FLOAT_EQ(s.horizontalAngle.ToFloat(), 0.0F) << "s.hor 0 1 0"; EXPECT_FLOAT_EQ(s.horizontal.ToFloat(), 0.0F) << "s.hor 0 1 0";
EXPECT_FLOAT_EQ(s.verticalAngle.ToFloat(), 90.0F) << "s.vert 0 1 0"; EXPECT_FLOAT_EQ(s.vertical.ToFloat(), 90.0F) << "s.vert 0 1 0";
v = Vector3(1, 0, 0); v = Vector3(1, 0, 0);
s = Spherical16(v); s = Spherical16::FromVector3(v);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0"; EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance 1 0 0";
EXPECT_FLOAT_EQ(s.horizontalAngle.ToFloat(), 90.0F) << "s.hor 1 0 0"; EXPECT_FLOAT_EQ(s.horizontal.ToFloat(), 90.0F) << "s.hor 1 0 0";
EXPECT_FLOAT_EQ(s.verticalAngle.ToFloat(), 0.0F) << "s.vert 1 0 0"; EXPECT_FLOAT_EQ(s.vertical.ToFloat(), 0.0F) << "s.vert 1 0 0";
} }
TEST(Spherical16, FromPolar) { // TEST(Spherical16, FromPolar) {
Polar p = Polar(1, 0); // Polar p = Polar(1, 0);
Spherical16 s = Spherical16(p); // Spherical16 s = Spherical16::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)"; // EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 0)";
EXPECT_FLOAT_EQ(s.horizontalAngle.ToFloat(), 0.0F) << "s.hor Polar(1 0)"; // EXPECT_FLOAT_EQ(s.horizontal.ToFloat(), 0.0F) << "s.hor Polar(1 0)";
EXPECT_FLOAT_EQ(s.verticalAngle.ToFloat(), 0.0F) << "s.vert Polar(1 0)"; // EXPECT_FLOAT_EQ(s.vertical.ToFloat(), 0.0F) << "s.vert Polar(1 0)";
p = Polar(1, 45); // p = Polar(1, 45);
s = Spherical16(p); // s = Spherical16::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)"; // EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 45)";
EXPECT_FLOAT_EQ(s.horizontalAngle.ToFloat(), 45.0F) << "s.hor Polar(1 45)"; // EXPECT_FLOAT_EQ(s.horizontal.ToFloat(), 45.0F) << "s.hor Polar(1 45)";
EXPECT_FLOAT_EQ(s.verticalAngle.ToFloat(), 0.0F) << "s.vert Polar(1 45)"; // EXPECT_FLOAT_EQ(s.vertical.ToFloat(), 0.0F) << "s.vert Polar(1 45)";
p = Polar(1, -45); // p = Polar(1, -45);
s = Spherical16(p); // s = Spherical16::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)"; // EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(1 -45)";
EXPECT_FLOAT_EQ(s.horizontalAngle.ToFloat(), -45.0F) << "s.hor Polar(1 -45)"; // EXPECT_FLOAT_EQ(s.horizontal.ToFloat(), -45.0F) << "s.hor Polar(1 -45)";
EXPECT_FLOAT_EQ(s.verticalAngle.ToFloat(), 0.0F) << "s.vert Polar(1 -45)"; // EXPECT_FLOAT_EQ(s.vertical.ToFloat(), 0.0F) << "s.vert Polar(1 -45)";
p = Polar(0, 0); // p = Polar(0, 0);
s = Spherical16(p); // s = Spherical16::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)"; // EXPECT_FLOAT_EQ(s.distance, 0.0F) << "s.distance Polar(0 0)";
EXPECT_FLOAT_EQ(s.horizontalAngle.ToFloat(), 0.0F) << "s.hor Polar(0 0)"; // EXPECT_FLOAT_EQ(s.horizontal.ToFloat(), 0.0F) << "s.hor Polar(0 0)";
EXPECT_FLOAT_EQ(s.verticalAngle.ToFloat(), 0.0F) << "s.vert Polar(0 0)"; // EXPECT_FLOAT_EQ(s.vertical.ToFloat(), 0.0F) << "s.vert Polar(0 0)";
p = Polar(-1, 0); // p = Polar(-1, 0);
s = Spherical16(p); // s = Spherical16::FromPolar(p);
EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)"; // EXPECT_FLOAT_EQ(s.distance, 1.0F) << "s.distance Polar(-1 0)";
EXPECT_FLOAT_EQ(s.horizontalAngle.ToFloat(), -180.0F) << "s.hor Polar(-1 0)"; // EXPECT_FLOAT_EQ(s.horizontal.ToFloat(), -180.0F) << "s.hor Polar(-1 0)";
EXPECT_FLOAT_EQ(s.verticalAngle.ToFloat(), 0.0F) << "s.vert Polar(-1 0)"; // EXPECT_FLOAT_EQ(s.vertical.ToFloat(), 0.0F) << "s.vert Polar(-1 0)";
} // }
TEST(Spherical16, Incident1) { TEST(Spherical16, Incident1) {
Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f); Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
Spherical16 s = Spherical16(v); Spherical16 s = Spherical16::FromVector3(v);
Spherical16 sr = Spherical16(2.49F, 98.18f, 24.4F); Spherical16 sr = Spherical16(2.49F, 98.18f, 24.4F);
EXPECT_NEAR(s.distance, sr.distance, 1.0e-01); EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
EXPECT_NEAR(s.horizontalAngle.ToFloat(), sr.horizontalAngle.ToFloat(), EXPECT_NEAR(s.horizontal.ToFloat(), sr.horizontal.ToFloat(), 1.0e-02);
1.0e-02); EXPECT_NEAR(s.vertical.ToFloat(), sr.vertical.ToFloat(), 1.0e-02);
EXPECT_NEAR(s.verticalAngle.ToFloat(), sr.verticalAngle.ToFloat(), 1.0e-02);
Vector3 r = Vector3(Spherical(sr.distance, sr.horizontalAngle.ToFloat(), Vector3 r = Spherical16(sr.distance, sr.horizontal, sr.vertical).ToVector3();
sr.verticalAngle.ToFloat()));
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-02) << "toVector3.x 1 0 0"; 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.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"; EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-02) << "toVector3.z 1 0 0";
@ -86,43 +84,38 @@ TEST(Spherical16, Incident1) {
TEST(Spherical16, Incident2) { TEST(Spherical16, Incident2) {
Vector3 v = Vector3(1.0f, 0.0f, 1.0f); Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
Spherical16 s = Spherical16(v); Spherical16 s = Spherical16::FromVector3(v);
Spherical16 sr = Spherical16(1.4142135623F, 45.0f, 0.0F); Spherical16 sr = Spherical16(1.4142135623F, 45.0f, 0.0F);
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05); EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.horizontalAngle.ToFloat(), sr.horizontalAngle.ToFloat(), EXPECT_NEAR(s.horizontal.ToFloat(), sr.horizontal.ToFloat(), 1.0e-05);
1.0e-05); EXPECT_NEAR(s.vertical.ToFloat(), sr.vertical.ToFloat(), 1.0e-05);
EXPECT_NEAR(s.verticalAngle.ToFloat(), sr.verticalAngle.ToFloat(), 1.0e-05);
Vector3 r = Vector3(Spherical(sr.distance, sr.horizontalAngle.ToFloat(), Vector3 r = Spherical16(sr.distance, sr.horizontal, sr.vertical).ToVector3();
sr.verticalAngle.ToFloat()));
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06); EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06); EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
v = Vector3(0.0f, 1.0f, 1.0f); v = Vector3(0.0f, 1.0f, 1.0f);
s = Spherical16(v); s = Spherical16::FromVector3(v);
sr = Spherical16(1.4142135623F, 0.0f, 45.0F); sr = Spherical16(1.4142135623F, 0.0f, 45.0F);
EXPECT_NEAR(s.distance, sr.distance, 1.0e-05); EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.horizontalAngle.ToFloat(), sr.horizontalAngle.ToFloat(), EXPECT_NEAR(s.horizontal.ToFloat(), sr.horizontal.ToFloat(), 1.0e-05);
1.0e-05); EXPECT_NEAR(s.vertical.ToFloat(), sr.vertical.ToFloat(), 1.0e-05);
EXPECT_NEAR(s.verticalAngle.ToFloat(), sr.verticalAngle.ToFloat(), 1.0e-05);
r = Vector3(Spherical(sr.distance, sr.horizontalAngle.ToFloat(), r = Spherical16(sr.distance, sr.horizontal, sr.vertical).ToVector3();
sr.verticalAngle.ToFloat()));
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06); EXPECT_NEAR(r.Right(), v.Right(), 1.0e-06);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06); EXPECT_NEAR(r.Up(), v.Up(), 1.0e-06);
EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06); EXPECT_NEAR(r.Forward(), v.Forward(), 1.0e-06);
v = Vector3(1.0f, 1.0f, 1.0f); v = Vector3(1.0f, 1.0f, 1.0f);
s = Spherical16(v); s = Spherical16::FromVector3(v);
r = Vector3(Spherical(s.distance, s.horizontalAngle.ToFloat(), r = Spherical16(s.distance, s.horizontal, s.vertical).ToVector3();
s.verticalAngle.ToFloat()));
EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02); EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
EXPECT_NEAR(s.horizontalAngle.ToFloat(), 45.0F, 1.0e-02); EXPECT_NEAR(s.horizontal.ToFloat(), 45.0F, 1.0e-02);
EXPECT_NEAR(s.verticalAngle.ToFloat(), 35.26F, 1.0e-02); EXPECT_NEAR(s.vertical.ToFloat(), 35.26F, 1.0e-02);
EXPECT_NEAR(r.Right(), v.Right(), 1.0e-04); EXPECT_NEAR(r.Right(), v.Right(), 1.0e-04);
EXPECT_NEAR(r.Up(), v.Up(), 1.0e-04); EXPECT_NEAR(r.Up(), v.Up(), 1.0e-04);
@ -150,14 +143,14 @@ TEST(Spherical16, Addition) {
v2 = Spherical16(1, -45, 0); v2 = Spherical16(1, -45, 0);
r = v1 + v2; r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)"; EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.horizontalAngle.ToFloat(), 0) << "Addition(1 -45 0)"; EXPECT_FLOAT_EQ(r.horizontal.ToFloat(), 0) << "Addition(1 -45 0)";
EXPECT_FLOAT_EQ(r.verticalAngle.ToFloat(), 0) << "Addition(1 -45 0)"; EXPECT_FLOAT_EQ(r.vertical.ToFloat(), 0) << "Addition(1 -45 0)";
v2 = Spherical16(1, 0, 90); v2 = Spherical16(1, 0, 90);
r = v1 + v2; r = v1 + v2;
EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)"; EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(1 0 90)";
EXPECT_FLOAT_EQ(r.horizontalAngle.ToFloat(), 45) << "Addition(1 0 90)"; EXPECT_FLOAT_EQ(r.horizontal.ToFloat(), 45) << "Addition(1 0 90)";
EXPECT_FLOAT_EQ(r.verticalAngle.ToFloat(), 45) << "Addition(1 0 90)"; EXPECT_FLOAT_EQ(r.vertical.ToFloat(), 45) << "Addition(1 0 90)";
} }
#endif #endif