quaternion.hh

#ifndef VTESTBED_GEOMETRY_QUATERNION_HH
#define VTESTBED_GEOMETRY_QUATERNION_HH

#include <iosfwd>

#include <vtestbed/base/bstream.hh>
#include <vtestbed/geometry/math.hh>
#include <vtestbed/geometry/types.hh>

namespace vtb {

    namespace geometry {

        template <class T>
        class Quaternion {

        public:
            using vec4 = Vertex<T,4>;
            using vec3 = Vertex<T,3>;

        private:
            vec4 _data{1,0,0,0};

        public:
            inline Quaternion(): _data{1,0,0,0} {}
            inline Quaternion(T s, T x, T y, T z): _data{s,x,y,z} {}
            inline Quaternion(const vec4& q): _data{q(0),q(1),q(2),q(3)} {}
            inline Quaternion(T re, const vec3& im): _data{re,im(0),im(1),im(2)} {}
            inline T w() const { return this->_data[0]; }
            inline T x() const { return this->_data[1]; }
            inline T y() const { return this->_data[2]; }
            inline T z() const { return this->_data[3]; }
            inline void clear() { *this = Quaternion(); }
            inline T real() const { return w(); }
            inline vec3 imag() const { return {x(),y(),z()}; }
            inline T operator()(int i) const { return this->_data[i]; }
            inline T operator[](int i) const { return this->_data[i]; }
            inline const vec4& data() const { return this->_data; }

            static Quaternion exp(const vec3& v);

            inline friend vtb::base::bstream&
            operator<<(vtb::base::bstream& out, const Quaternion& rhs) {
                out << rhs._data;
                return out;
            }

            inline friend vtb::base::bstream&
            operator>>(vtb::base::bstream& in, Quaternion& rhs) {
                in >> rhs._data;
                return in;
            }

        };

        template <class T>
        std::ostream&
        operator<<(std::ostream& out, const Quaternion<T>& q);

        template <class T> inline T real(const Quaternion<T>& q) { return q.real(); }
        template <class T> inline Vertex<T,3> imag(const Quaternion<T>& q) { return q.imag(); }

        template <class T>
        Quaternion<T>
        operator*(const Quaternion<T>& a, const Quaternion<T>& b);

        template <class T>
        Quaternion<T>
        operator*=(Quaternion<T>& a, const Quaternion<T>& b) {
            a = a*b;
            return a;
        }

        template <class T>
        inline Quaternion<T>
        operator*(const Quaternion<T>& a, T b) {
            return {a(0)*b, a(1)*b, a(2)*b, a(3)*b};
        }

        template <class T>
        inline Quaternion<T>
        operator*(T a, const Quaternion<T>& b) {
            return {a*b(0), a*b(1), a*b(2), a*b(3)};
        }

        template <class T>
        inline Quaternion<T>
        operator/(const Quaternion<T>& a, T b) {
            return {a(0)/b, a(1)/b, a(2)/b, a(3)/b};
        }

        template <class T>
        Quaternion<T>
        operator+(Quaternion<T>& a, const Quaternion<T>& b) {
            return Quaternion<T>{a(0)+b(0),a(1)+b(1),a(2)+b(2),a(3)+b(3)};
        }

        template <class T>
        Quaternion<T>
        operator+=(Quaternion<T>& a, const Quaternion<T>& b) {
            a = a+b;
            return a;
        }

        template <class T>
        inline Quaternion<T>
        unit(const Quaternion<T>& q) {
            return ::vtb::geometry::unit(q.data());
        }

        template <class T>
        inline T
        length(const Quaternion<T>& q) {
            return blitz::length(q.data());
        }

        template <class T>
        Vertex<T,3>
        to_euler_angles(const Quaternion<T>& q);

        template <class T>
        Quaternion<T>
        from_euler_angles(const Vertex<T,3>& angles);

        template <class T>
        Rotation_matrix<T,3>
        rotation_matrix(const Quaternion<T>& q);

        template <class T>
        inline Quaternion<T>
        from_vector(const Quaternion<T>& q) {
            using blitz::pow2;
            using std::sqrt;
            return Quaternion<T>{
                sqrt(1 - pow2(q(1)) - pow2(q(2)) - pow2(q(3))),
                q(1),
                q(2),
                q(3)
            };
        }

        template <class T>
        inline Quaternion<T>
        to_vector(const Quaternion<T>& q) {
            return Quaternion<T>{0, q(1), q(2), q(3)};
        }

        template <class T>
        inline Quaternion<T>
        conj(const Quaternion<T>& q) {
            return Quaternion<T>{q(0), -q(1), -q(2), -q(3)};
        }

        template <class T>
        inline Vertex<T,3>
        rotate(const Vertex<T,3>& v, const Quaternion<T>& q) {
            return (q*Quaternion<T>(T{},v)*conj(q)).imag();
        }

        template <class T>
        class Quaternion_coordinate_system {
        public:
            using scalar_type = T;
            using vertex_type = Vertex<T,3>;
            using quaternion_type = Quaternion<T>;

        private:
            quaternion_type _quaternion, _quaternion_conj;
            vertex_type _origin{T{}};

        public:

            Quaternion_coordinate_system() = default;

            inline
            Quaternion_coordinate_system(const quaternion_type& q,
                                         const quaternion_type& q_conj,
                                         const vertex_type& origin):
                _quaternion(q), _quaternion_conj(q_conj), _origin(origin) {}

            inline
            Quaternion_coordinate_system(const quaternion_type& q, const vertex_type& origin):
            Quaternion_coordinate_system(q, conj(q), origin) {}

            inline const quaternion_type& quaternion() const { return this->_quaternion; }
            inline const quaternion_type& quaternion_conj() const { return this->_quaternion_conj; }

            inline void
            quaternion(const quaternion_type& rhs) {
                this->_quaternion = rhs, this->_quaternion_conj = conj(rhs);
            }

            inline const vertex_type& origin() const { return this->_origin; }
            inline void origin(const vertex_type& rhs) { this->_origin = rhs; }

            inline void clear() {
                this->_quaternion.clear(), this->_quaternion_conj.clear(),
                this->_origin = T{};
            }

        };

        template <class T>
        inline Quaternion_coordinate_system<T>
        make_coordinate_system(const Quaternion<T>& q, const Vertex<T,3>& origin) {
            return Quaternion_coordinate_system<T>(q, origin);
        }

        template <class T>
        inline Quaternion_coordinate_system<T>
        make_coordinate_system(const Quaternion<T>& q,
                               const Quaternion<T>& q_conj,
                               const Vertex<T,3>& origin) {
            return Quaternion_coordinate_system<T>(q, q_conj, origin);
        }

        template <class T>
        inline Vertex<T,3>
        to(const Quaternion_coordinate_system<T>& cs, const Vertex<T,3>& v) {
            Vertex<T,3> x = v-cs.origin();
            return rotate(x, cs.quaternion());
        }

        template <class T>
        inline Vertex<T,3>
        from(const Quaternion_coordinate_system<T>& cs, const Vertex<T,3>& v) {
            return rotate(v, cs.quaternion_conj()) + cs.origin();
        }

        template <class T>
        inline Vertex<T,3>
        vector_to(const Quaternion_coordinate_system<T>& cs, const Vertex<T,3>& v) {
            return rotate(v, cs.quaternion());
        }

        template <class T>
        inline Vertex<T,3>
        vector_from(const Quaternion_coordinate_system<T>& cs, const Vertex<T,3>& v) {
            return rotate(v, cs.quaternion_conj());
        }

    }

}

#endif // vim:filetype=cpp