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

Further vector3 conversion fixes

Browse Source
This commit is contained in:
Pascal Serrarens 2024-03-15 17:20:21 +01:00
parent ec0cc47846
commit ae55a24dc0
2 changed files with 30 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Spherical.cpp
View File

@ -13,13 +13,6 @@ Spherical::Spherical() {
this->distance = 0; this->distance = 0;
} }
// Spherical::Spherical(float polarAngle, float elevationAngle, float distance)
// {
// this->horizontalAngle = polarAngle;
// this->verticalAngle = elevationAngle;
// this->distance = distance;
// }
Spherical::Spherical(Polar polar) { Spherical::Spherical(Polar polar) {
this->horizontalAngle = polar.angle; this->horizontalAngle = polar.angle;
this->verticalAngle = 0.0F; this->verticalAngle = 0.0F;
@ -32,8 +25,13 @@ Spherical::Spherical(float distance, Angle horizontalAngle, Angle verticalAngle)
Spherical::Spherical(Vector3 v) { Spherical::Spherical(Vector3 v) {
distance = v.magnitude(); distance = v.magnitude();
verticalAngle = 90 - acosf(v.y / distance) * Angle::Rad2Deg; if (distance == 0.0f) {
horizontalAngle = atanf(v.x / v.z) * Angle::Rad2Deg; verticalAngle = 0;
horizontalAngle = 0;
} else {
verticalAngle = (90 - acosf(v.y / distance) * Angle::Rad2Deg);
horizontalAngle = atan2f(v.x, v.z) * Angle::Rad2Deg;
}
} }
const Spherical Spherical::zero = Spherical(0.0F, (Angle)0.0F, (Angle)0.0F); const Spherical Spherical::zero = Spherical(0.0F, (Angle)0.0F, (Angle)0.0F);

39
test/Spherical_test.cc
View File

@ -36,7 +36,7 @@ TEST(Spherical, ToVector3) {
Vector3 r = s.ToVector3(); Vector3 r = s.ToVector3();
EXPECT_FLOAT_EQ(r.x, 0.0F) << "toVector3.x 0 0 1"; EXPECT_FLOAT_EQ(r.x, 0.0F) << "toVector3.x 0 0 1";
EXPECT_FLOAT_EQ(r.y, 0.0F) << "toVector3.y 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"; EXPECT_FLOAT_EQ(r.z, 1.0F) << "toVector3.z 0 0 1";
v = Vector3(0, 1, 0); v = Vector3(0, 1, 0);
@ -52,41 +52,48 @@ TEST(Spherical, ToVector3) {
r = s.ToVector3(); r = s.ToVector3();
EXPECT_FLOAT_EQ(r.x, 1.0F) << "toVector3.x 1 0 0"; EXPECT_FLOAT_EQ(r.x, 1.0F) << "toVector3.x 1 0 0";
EXPECT_FLOAT_EQ(r.y, 0.0F) << "toVector3.y 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"; EXPECT_NEAR(r.z, 0.0F, 1.0e-06) << "toVector3.z 1 0 0";
} }
TEST(Spherical, Incident1) { TEST(Spherical, Incident1) {
Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f); Vector3 v = Vector3(2.242557f, 1.027884f, -0.322347f);
Spherical s = Spherical(v); Spherical s = Spherical(v);
Vector3 r = s.ToVector3();
EXPECT_NEAR(r.x, v.x, 1.0e-06) << "toVector3.x 1 0 0"; Spherical sr = Spherical(2.49F, 98.18f, 24.4F);
EXPECT_NEAR(r.y, v.y, 1.0e-06) << "toVector3.y 1 0 0"; EXPECT_NEAR(s.distance, sr.distance, 1.0e-01);
EXPECT_NEAR(r.z, v.z, 1.0e-06) << "toVector3.z 1 0 0"; 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) { TEST(Spherical, Incident2) {
Vector3 v = Vector3(1.0f, 0.0f, 1.0f); Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
Spherical s = Spherical(v); Spherical s = Spherical(v);
Vector3 r = s.ToVector3();
EXPECT_NEAR(s.distance, 1.4142135623F, 1.0e-05); Spherical sr = Spherical(1.4142135623F, 45.0f, 0.0F);
EXPECT_NEAR(s.horizontalAngle, 45.0F, 1.0e-05); EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.verticalAngle, 0.0F, 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.x, v.x, 1.0e-06);
EXPECT_NEAR(r.y, v.y, 1.0e-06); EXPECT_NEAR(r.y, v.y, 1.0e-06);
EXPECT_NEAR(r.z, v.z, 1.0e-06); EXPECT_NEAR(r.z, v.z, 1.0e-06);
v = Vector3(0.0f, 1.0f, 1.0f); v = Vector3(0.0f, 1.0f, 1.0f);
s = Spherical(v); s = Spherical(v);
r = s.ToVector3();
EXPECT_NEAR(s.distance, 1.4142135623F, 1.0e-05); sr = Spherical(1.4142135623F, 0.0f, 45.0F);
EXPECT_NEAR(s.horizontalAngle, 0.0F, 1.0e-05); EXPECT_NEAR(s.distance, sr.distance, 1.0e-05);
EXPECT_NEAR(s.verticalAngle, 45.0F, 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.x, v.x, 1.0e-06);
EXPECT_NEAR(r.y, v.y, 1.0e-06); EXPECT_NEAR(r.y, v.y, 1.0e-06);
EXPECT_NEAR(r.z, v.z, 1.0e-06); EXPECT_NEAR(r.z, v.z, 1.0e-06);
@ -95,9 +102,9 @@ TEST(Spherical, Incident2) {
s = Spherical(v); s = Spherical(v);
r = s.ToVector3(); r = s.ToVector3();
EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-06); EXPECT_NEAR(s.distance, 1.73205080F, 1.0e-02);
EXPECT_NEAR(s.horizontalAngle, 45.0F, 1.0e-02); EXPECT_NEAR(s.horizontalAngle, 45.0F, 1.0e-02);
// EXPECT_NEAR(s.verticalAngle, 35.26F, 1.0e-06); EXPECT_NEAR(s.verticalAngle, 35.26F, 1.0e-02);
EXPECT_NEAR(r.x, v.x, 1.0e-06); EXPECT_NEAR(r.x, v.x, 1.0e-06);
EXPECT_NEAR(r.y, v.y, 1.0e-06); EXPECT_NEAR(r.y, v.y, 1.0e-06);