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

Fix spherical to vector

Browse Source
This commit is contained in:
Pascal Serrarens 2024-03-15 15:36:52 +01:00
parent 5141766de5
commit ec0cc47846
2 changed files with 64 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
Spherical.cpp
View File

@ -31,11 +31,9 @@ Spherical::Spherical(float distance, Angle horizontalAngle, Angle verticalAngle)
verticalAngle(verticalAngle) {}
Spherical::Spherical(Vector3 v) {
float signZ = (v.z >= 0) - (v.z < 0);
verticalAngle =
atan2(v.y, signZ * sqrt(v.z * v.z + v.x * v.x)) * Angle::Rad2Deg;
horizontalAngle = atan2(v.x, sqrt(v.z * v.z + v.y * v.y)) * Angle::Rad2Deg;
distance = v.magnitude();
verticalAngle = 90 - acosf(v.y / distance) * Angle::Rad2Deg;
horizontalAngle = atanf(v.x / v.z) * Angle::Rad2Deg;
}
const Spherical Spherical::zero = Spherical(0.0F, (Angle)0.0F, (Angle)0.0F);
@ -51,13 +49,14 @@ Polar Spherical::ProjectOnHorizontalPlane() {
}
Vector3 Spherical::ToVector3() {
// Vector3 v = Quaternion::AngleAxis(this->horizontalAngle, Vector3::up) *
// Quaternion::AngleAxis(this->verticalAngle, Vector3::left) *
// Vector3::forward * this->distance;
float verticalRad = verticalAngle * Angle::Deg2Rad;
float verticalRad = (90 - verticalAngle) * Angle::Deg2Rad;
float horizontalRad = horizontalAngle * Angle::Deg2Rad;
Vector3 v = Vector3(this->distance * cosf(verticalRad) * sinf(horizontalRad),
this->distance * sinf(verticalRad),
this->distance * cosf(verticalRad) * cosf(horizontalRad));
float cosVertical = cosf(verticalRad);
float sinVertical = sinf(verticalRad);
float cosHorizontal = cosf(horizontalRad);
float sinHorizontal = sinf(horizontalRad);
Vector3 v = Vector3(this->distance * sinVertical * sinHorizontal,
this->distance * cosVertical,
this->distance * sinVertical * cosHorizontal);
return v;
}

54
test/Spherical_test.cc
View File

@ -55,4 +55,58 @@ TEST(Spherical, ToVector3) {
EXPECT_FLOAT_EQ(r.y, 0.0F) << "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);
Vector3 r = s.ToVector3();
EXPECT_NEAR(r.x, v.x, 1.0e-06) << "toVector3.x 1 0 0";
EXPECT_NEAR(r.y, v.y, 1.0e-06) << "toVector3.y 1 0 0";
EXPECT_NEAR(r.z, v.z, 1.0e-06) << "toVector3.z 1 0 0";
}
TEST(Spherical, Incident2) {
Vector3 v = Vector3(1.0f, 0.0f, 1.0f);
Spherical s = Spherical(v);
Vector3 r = s.ToVector3();
EXPECT_NEAR(s.distance, 1.4142135623F, 1.0e-05);
EXPECT_NEAR(s.horizontalAngle, 45.0F, 1.0e-05);
EXPECT_NEAR(s.verticalAngle, 0.0F, 1.0e-05);
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);
r = s.ToVector3();
EXPECT_NEAR(s.distance, 1.4142135623F, 1.0e-05);
EXPECT_NEAR(s.horizontalAngle, 0.0F, 1.0e-05);
EXPECT_NEAR(s.verticalAngle, 45.0F, 1.0e-05);
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-06);
EXPECT_NEAR(s.horizontalAngle, 45.0F, 1.0e-02);
// EXPECT_NEAR(s.verticalAngle, 35.26F, 1.0e-06);
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