diff --git a/vector.hpp b/vector.hpp index 4b2f5885..8763bbef 100644 --- a/vector.hpp +++ b/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 + 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 vector<2,T> polar_to_cartesian (vector<2,T>); template vector<2,T> cartesian_to_polar (vector<2,T>);