vector: add make_basis function

vector.hpp

@@ -79,6 +79,38 @@ namespace util {
         return a[0] * b[1] - a[1] * b[0];
     }
 
+
+    //-------------------------------------------------------------------------
+    // given a vector find two vectors which produce an orthonormal basis.
+    //
+    // we use frisvad's method, avoids explicit normalisation. a good
+    // alternative is hughes-moeller, but the paper is hard to find.
+    template <typename T>
+    std::pair<
+        util::vector<3,T>,
+        util::vector<3,T>
+    >
+    make_basis (const util::vector<3,T> n)
+    {
+
+        // avoid a singularity
+        if (n.z < -T(0.9999999)) {
+            return {
+                {  0, -1, 0 },
+                { -1, -1, 0 }
+            };
+        }
+
+        const T a = 1 / (1 + n.z);
+        const T b = -n.x * n.y * a;
+
+        return {
+            { 1 - n.x * n.x * a, b, -n.x },
+            { b, 1 - n.y * n.y * a, -n.y }
+        };
+    }
+
+
     // polar/cartesian conversions; assumes (mag, angle) form.
     template <typename T> vector<2,T> polar_to_cartesian (vector<2,T>);
     template <typename T> vector<2,T> cartesian_to_polar (vector<2,T>);