From 130b7efc23b3883a17395cf5e92e4847aa7bd4e6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Erwan=20de=20L=C3=A9pinau?= <erwan@feedly.com>
Date: Thu, 16 Apr 2020 23:15:54 -0700
Subject: [PATCH] implement spherical rotation for Vector3

---
 src/vectors.cpp        | 12 ++++++++++++
 src/vectors.hpp        |  2 ++
 tests/test_vectors.cpp | 18 ++++++++++++++++++
 3 files changed, 32 insertions(+)

diff --git a/src/vectors.cpp b/src/vectors.cpp
index 08ff1c6..64408c0 100644
--- a/src/vectors.cpp
+++ b/src/vectors.cpp
@@ -34,6 +34,7 @@ Vector3 &Vector3::operator/=(float f) {
     z /= f;
     return *this;
 }
+
 Vector3 Vector3::operator-() const { return Vector3(-x, -y, -z); }
 
 bool Vector3::operator==(const Vector3 &other) const {
@@ -97,6 +98,17 @@ Vector3 Vector3::reflected(const Vector3 &normal) const {
     return *this - 2 * dot(normalizedNormal) * normalizedNormal;
 }
 
+Vector3 Vector3::sphericallyRotated(const Vector3 &normalizedDirection,
+                                    float theta, float phi) {
+    float originalTheta =
+        std::atan2(normalizedDirection.getY(), normalizedDirection.getX());
+    float originalPhi = std::acos(normalizedDirection.getZ());
+    float newX = std::sin(originalPhi + phi) * std::cos(originalTheta + theta);
+    float newY = std::sin(originalPhi + phi) * std::sin(originalTheta + theta);
+    float newZ = std::cos(originalPhi + phi);
+    return {newX, newY, newZ};
+}
+
 float Vector3::getX() const { return x; }
 float Vector3::getY() const { return y; }
 float Vector3::getZ() const { return z; }
diff --git a/src/vectors.hpp b/src/vectors.hpp
index 96a5bfc..f277371 100644
--- a/src/vectors.hpp
+++ b/src/vectors.hpp
@@ -18,6 +18,8 @@ class Vector3 {
     float dot(const Vector3 &other) const;
     Vector3 cross(const Vector3 &other) const;
     Vector3 reflected(const Vector3 &normal) const;
+    static Vector3 sphericallyRotated(const Vector3 &normalizedDirection,
+                                      float theta, float phi);
 
     Vector3 &operator+=(const Vector3 &other);
     Vector3 &operator-=(const Vector3 &other);
diff --git a/tests/test_vectors.cpp b/tests/test_vectors.cpp
index e830e5f..aca11cf 100644
--- a/tests/test_vectors.cpp
+++ b/tests/test_vectors.cpp
@@ -1,6 +1,7 @@
 #include <catch2/catch.hpp>
 #include <cmath>
 
+#include "../src/utils.hpp"
 #include "../src/vectors.hpp"
 
 TEST_CASE("Vector2") {
@@ -93,4 +94,21 @@ TEST_CASE("Vector3") {
 
         REQUIRE(reflected == Vector3(1.0f, 1.0f, 1.0f));
     }
+
+    SECTION("sphericallyRotated returns correct results") {
+        const Vector3 v1{1.0f, 1.0f, std::sqrt(2.0f)};
+
+        const Vector3 rotated1 = Vector3::sphericallyRotated(
+            v1.normalized(), -Math::PI / 4.0f, Math::PI / 4.0f);
+
+        REQUIRE(rotated1 == Vector3(1.0f, 0.0f, 0.0f).normalized());
+
+        const Vector3 v3{-1.0f, 0.0f, 0.0f};
+
+        const Vector3 rotated2 = Vector3::sphericallyRotated(
+            v3.normalized(), Math::PI / 2.0f, Math::PI / 6.0f);
+
+        REQUIRE(rotated2 ==
+                Vector3(0.0f, -std::sqrt(3.0f) / 2.0f, -0.5f).normalized());
+    }
 }
\ No newline at end of file