From 51ffcbb07e97f0863142e00925a3afcfc44f77db Mon Sep 17 00:00:00 2001
From: Danny Robson <danny@nerdcruft.net>
Date: Wed, 17 Jan 2018 17:44:59 +1100
Subject: [PATCH] vector: add spherical canonicalisation function

---
 vector.hpp | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/vector.hpp b/vector.hpp
index ed732543..29c3aece 100644
--- a/vector.hpp
+++ b/vector.hpp
@@ -20,6 +20,7 @@
 #include "./coord/fwd.hpp"
 #include "./coord.hpp"
 
+#include "maths.hpp"
 #include "json/fwd.hpp"
 
 #include <cstddef>
@@ -84,6 +85,26 @@ namespace util {
         };
     }
 
+    template <typename T>
+    constexpr vector<3,T>
+    canonical_spherical (vector<3,T> s)
+    {
+        if (s.x < 0) {
+            s.x  = -s.x;
+            s.y += util::PI<T>;
+        }
+
+        if (s.y < 0) {
+            s.y = -s.y;
+            s.z += util::PI<T>;
+        }
+
+        s.y = std::fmod (s.y, util::PI<T>);
+        s.z = std::fmod (s.z, util::PI<T>);
+
+        return s;
+    }
+
     template <typename T> vector<2,T> to_euler   (vector<3,T>);
     template <typename T> vector<3,T> from_euler (vector<2,T>);