basis.hh

#ifndef VTESTBED_GEOMETRY_BASIS_HH
#define VTESTBED_GEOMETRY_BASIS_HH

#include <array>

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

namespace vtb {

    namespace geometry {

        using bstream = ::vtb::base::bstream;

        template <class T, int N>
        class Basis: public Matrix<T,N> {

        private:
            using matrix_type = Matrix<T,N>;
            using row_type = Vertex<T,N>;
            using column_type = Vertex<T,N>;
            using array_type = std::array<T,N*N>;

        public:
            using matrix_type::matrix_type;
            using matrix_type::operator=;
            using matrix_type::operator();

            inline Basis(): matrix_type(blitz::identity_matrix<T,N>()) {}
            inline Basis(const matrix_type& rhs): matrix_type(rhs) {}
            inline Basis(const array_type& rhs): matrix_type(blitz::const_matrix<T,N>(rhs)) {}

            inline Basis&
            operator=(const array_type& rhs) {
                *this = blitz::const_matrix<T,N>(rhs);
                return *this;
            }

            inline column_type
            row(int n) const {
                const auto& m = *this;
                row_type r;
                for (int i=0; i<N; ++i) { r(i) = m(n,i); }
                return r;
            }

            inline column_type
            column(int n) const {
                const auto& m = *this;
                column_type c;
                for (int i=0; i<N; ++i) { c(i) = m(i,n); }
                return c;
            }

            template <class ... Args>
            inline Basis<T,N>
            column_basis(Args ... cols) const {
                const int columns[sizeof...(cols)] = {cols...};
                const auto& m = *this;
                Basis<T,N> b{T{}};
                for (int i=0; i<N; ++i) { b(i,i) = 1; }
                for (int i=0; i<N; ++i) {
                    for (const auto& j : columns) {
                        b(i,j) = m(i,j);
                    }
                }
                return b;
            }

            inline void clear() { *this = blitz::identity_matrix<T,N>(); }

        };

        template <class T, int N>
        inline void
        transpose(Basis<T,N>& basis) {
            blitz::transpose(basis);
        }

        template <class T, int N>
        inline Vertex<T,N>
        to(const Basis<T,N>& b, const Vertex<T,N>& v) {
            return product(b, v);
        }

        template <class T, int N>
        Basis<T,N>
        rotation_matrix_zyx(const Vertex<T,N>& angle);

        template <class T, int N>
        Basis<T,N>
        rotation_matrix_xyz(const Vertex<T,N>& angle);

        template <class T, int N>
        Basis<T,N>
        inverse(const Basis<T,N>& basis);

        template <class T, int N>
        bstream&
        operator<<(bstream& out, const Rotation_matrix<T,N>& rhs);

        template <class T, int N>
        bstream&
        operator>>(bstream& in, Rotation_matrix<T,N>& rhs);

        template <class T, int N>
        class Rotation_matrix: public Basis<T,N> {

        private:
            using basis_type = Basis<T,N>;
            using vertex_type = Vertex<T,N>;

        private:
            basis_type _ibasis;

        public:
            Rotation_matrix() = default;

            inline explicit
            Rotation_matrix(const basis_type& basis, const basis_type& ibasis):
            basis_type(basis), _ibasis(ibasis) {}

            inline explicit
            Rotation_matrix(const basis_type& basis):
            Rotation_matrix(basis, inverse(basis)) {}

            inline const basis_type& inverse() const { return this->_ibasis; }
            inline void clear() { this->basis_type::clear(); this->_ibasis.clear(); }

            friend bstream& operator<< <T,N>(bstream& out, const Rotation_matrix<T,N>& rhs);
            friend bstream& operator>> <T,N>(bstream& in, Rotation_matrix<T,N>& rhs);

        };

        template <class T, int N>
        Basis<T,N>
        inverse(const Rotation_matrix<T,N>& rot) {
            return rot.inverse();
        }

        template <class T, int N, class X>
        inline auto
        operator*(const Basis<T,N>& lhs, const X& rhs) -> decltype(product(lhs, rhs)) {
            return product(lhs, rhs);
        }

        template <class T, int N>
        inline Vertex<T,N>
        to(const Rotation_matrix<T,N>& rot, const Vertex<T,N>& v) {
            return product(rot, v);
        }

        template <class T, int N>
        inline Vertex<T,N>
        from(const Rotation_matrix<T,N>& rot, const Vertex<T,N>& v) {
            return product(rot.inverse(), v);
        }

        template <class T, int N>
        inline Rotation_matrix<T,N>
        make_rotation(const Basis<T,N>& basis, const Basis<T,N>& ibasis) {
            return Rotation_matrix<T,N>(basis, ibasis);
        }

        template <class T, int N>
        inline Rotation_matrix<T,N>
        make_rotation(const Basis<T,N>& basis) {
            return Rotation_matrix<T,N>(basis);
        }

        template <class T, int N>
        Rotation_matrix<T,N>
        make_rotation_zyx(const Vertex<T,N>& angles);

        template <class T, int N>
        Rotation_matrix<T,N>
        make_rotation_xyz(const Vertex<T,N>& angles);

        template <class T, int N>
        bstream&
        operator<<(bstream& out, const Coordinate_system<T,N>& rhs);

        template <class T, int N>
        bstream&
        operator>>(bstream& in, Coordinate_system<T,N>& rhs);

        template <class T, int N>
        class Coordinate_system {

        private:
            using basis_type = Basis<T,N>;
            using vertex_type = Vertex<T,N>;
            using rotation_type = Rotation_matrix<T,N>;

        private:
            rotation_type _rotation;
            vertex_type _origin{T{}};

        public:

            Coordinate_system() = default;

            inline
            Coordinate_system(const basis_type& basis, const vertex_type& origin):
            _rotation(basis, inverse(basis)), _origin(origin) {}

            inline
            Coordinate_system(const basis_type& basis, const basis_type& ibasis,
                const vertex_type& origin): _rotation(basis, ibasis), _origin(origin) {}

            inline
            Coordinate_system(const rotation_type& rot, const vertex_type& origin):
            _rotation(rot), _origin(origin) {}

            inline const rotation_type& rotation() const { return this->_rotation; }
            inline void rotation(const rotation_type& rhs) { this->_rotation = rhs; }
            inline const basis_type& basis() const { return this->_rotation; }
            inline const basis_type& inverse_basis() const { return this->_rotation.inverse(); }
            inline const vertex_type& origin() const { return this->_origin; }
            inline void origin(const vertex_type& rhs) { this->_origin = rhs; }
            inline void clear() { this->_rotation.clear(); this->_origin = T{}; }

            friend bstream& operator<< <T,N>(bstream& out, const Coordinate_system<T,N>& rhs);
            friend bstream& operator>> <T,N>(bstream& in, Coordinate_system<T,N>& rhs);

        };

        template <class T, int N>
        inline Coordinate_system<T,N>
        make_coordinate_system(const Basis<T, N>& basis, const Vertex<T, N>& origin) {
            return Coordinate_system<T,N>(basis, origin);
        }

        template <class T, int N>
        inline Coordinate_system<T,N>
        make_coordinate_system(
            const Basis<T,N>& basis,
            const Basis<T,N>& ibasis,
            const Vertex<T,N>& origin
        ) {
            return Coordinate_system<T,N>(basis, ibasis, origin);
        }

        template <class T, int N>
        inline Coordinate_system<T,N>
        make_coordinate_system(const Rotation_matrix<T,N>& rot, const Vertex<T,N>& origin) {
            return Coordinate_system<T,N>(rot, origin);
        }

        template <class T, int N>
        inline Vertex<T,N>
        to(const Coordinate_system<T,N>& cs, const Vertex<T,N>& v) {
            Vertex<T,N> x = v-cs.origin();
            return product(cs.basis(), x);
        }

        template <class T, int N>
        inline Vertex<T,N>
        from(const Coordinate_system<T,N>& cs, const Vertex<T,N>& v) {
            return product(cs.inverse_basis(), v) + cs.origin();
        }

        template <class T, int N>
        inline Vertex<T,N>
        vector_to(const Rotation_matrix<T,N>& r, const Vertex<T,N>& v) {
            return product(r, v);
        }

        template <class T, int N>
        inline Vertex<T,N>
        vector_to(const Coordinate_system<T,N>& cs, const Vertex<T,N>& v) {
            return product(cs.basis(), v);
        }

        template <class T, int N>
        inline Vertex<T,N>
        vector_from(const Coordinate_system<T,N>& cs, const Vertex<T,N>& v) {
            return product(cs.inverse_basis(), v);
        }

        template <class T, int N>
        inline Vertex<T,N>
        from(
            const Basis<T,N>& basis,
            const Vertex<T,N>& origin,
            const Vertex<T,N>& v
        ) {
            return product(inverse(basis), v) + origin;
        }

    }

}

#endif // vim:filetype=cpp