From 5141766de58b4d988ff1e4eb6e83f32257034ee4 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Thu, 14 Mar 2024 15:27:34 +0100
Subject: [PATCH] Add spherical.toVector3 test (and fixes)

---
 Spherical.cpp          | 11 ++++++++---
 test/Spherical_test.cc | 26 ++++++++++++++++++++++++++
 2 files changed, 34 insertions(+), 3 deletions(-)

diff --git a/Spherical.cpp b/Spherical.cpp
index 7253a09..6de28a7 100644
--- a/Spherical.cpp
+++ b/Spherical.cpp
@@ -51,8 +51,13 @@ Polar Spherical::ProjectOnHorizontalPlane() {
 }
 
 Vector3 Spherical::ToVector3() {
-  Vector3 v = Quaternion::Euler(
-                  Vector3(-this->verticalAngle, this->horizontalAngle, 0)) *
-              Vector3::forward * this->distance;
+  // Vector3 v = Quaternion::AngleAxis(this->horizontalAngle, Vector3::up) *
+  //             Quaternion::AngleAxis(this->verticalAngle, Vector3::left) *
+  //             Vector3::forward * this->distance;
+  float verticalRad = 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));
   return v;
 }
\ No newline at end of file
diff --git a/test/Spherical_test.cc b/test/Spherical_test.cc
index 6f9c527..bea62e0 100644
--- a/test/Spherical_test.cc
+++ b/test/Spherical_test.cc
@@ -29,4 +29,30 @@ TEST(Spherical, FromVector3) {
   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_FLOAT_EQ(r.y, 0.0F) << "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_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";
+}
 #endif
\ No newline at end of file