三次元ジオメトリ計算用ETライブラリ

そろそろ日記書くべきだということで、はるか一年ぐらい前に書いたジオメトリ計算用ライブラリをちょこっと手直しして載せてみる。
もともと行き当たりばったりに書いていたものなので、とってもカオス。特に同次座標辺りはすごい適当な扱い。ほとんど計算に使われていない。

追記：さすがに適当すぎたのでgcc4.4でもテストして通るようにしてみた。
追記2：計算結果がそれっぽくなるように修正。

#if !defined NEWTYPE_HPP_INCLUDED_

#define      NEWTYPE_HPP_INCLUDED_
#include <stdexcept>

#include <cmath>

#include <boost/type_traits/is_integral.hpp>

#include <boost/type_traits/is_floating_point.hpp>

#include <boost/type_traits/is_arithmetic.hpp>

#include <boost/type_traits/is_pointer.hpp>

#include <boost/type_traits/is_base_of.hpp>

#include <boost/type_traits/promote.hpp>

#include <boost/type_traits/detail/yes_no_type.hpp>

#include <boost/utility/enable_if.hpp>

#include <boost/mpl/if.hpp>

#include <boost/mpl/placeholders.hpp>

#include <boost/mpl/or.hpp>

#include <boost/mpl/and.hpp>
namespace newtype {
    // forward declare

    template<typename Vec, typename Elem>

    struct vector_base;

    template<typename Elem>

    struct vector;

    template<typename Mat, typename Elem>

    struct matrix_base;

    template<typename Elem>

    struct matrix;

    template<typename Mat>

    class matrix_row;

    template<typename Mat>

    class matrix_column;
    // zero_vector

    struct zero_vector_type {

    } const zero_vector = {};
    struct identity_type {

    } const identity = {};
    struct zero_matrix_type {

    } const zero_matrix = {};
    namespace detail {

        template<typename T, typename U>

        struct is_base_template_of_impl;

        template<template<typename> class Base, typename Derived>

        struct is_base_template_of_impl<Base<boost::mpl::_>, Derived> {

            template<typename T1>

            static boost::type_traits::yes_type test(Base<T1> * p);

            static boost::type_traits::no_type test(...);

            static bool const value = sizeof(test(static_cast<Derived*>(0))) == sizeof(boost::type_traits::yes_type);

        };

        template<template<typename, typename> class Base, typename Derived>

        struct is_base_template_of_impl<Base<boost::mpl::_, boost::mpl::_>, Derived> {

            template<typename T1, typename T2>

            static boost::type_traits::yes_type test(Base<T1, T2> * p);

            static boost::type_traits::no_type test(...);

            static bool const value = sizeof(test(static_cast<Derived*>(0))) == sizeof(boost::type_traits::yes_type);

        };
        template<typename Base, typename Derived>

        struct is_base_template_of : public

            boost::mpl::if_c<is_base_template_of_impl<Base, Derived>::value,

                             boost::true_type,

                             boost::false_type>::type

        {};
        template<typename T>

        struct is_vector : public is_base_template_of<vector_base<boost::mpl::_, boost::mpl::_>, T>

        {};
        template<typename T>

        struct is_matrix : public is_base_template_of<matrix_base<boost::mpl::_, boost::mpl::_>, T>

        {};
        template<typename T>

        struct arithmetic_grade { static const int value = 0;};

        template<typename T> struct arithmetic_grade<T const> : public arithmetic_grade<T> {};

        template<typename T> struct arithmetic_grade<T volatile> : public arithmetic_grade<T> {};

        template<typename T> struct arithmetic_grade<T const volatile> : public arithmetic_grade<T> {};

        template<> struct arithmetic_grade<long double> { static const int value = 4; };

        template<> struct arithmetic_grade<double> { static const int value = 3; };

        template<> struct arithmetic_grade<float> { static const int value = 3; };

        template<> struct arithmetic_grade<long> { static const int value = 2; };

        template<> struct arithmetic_grade<unsigned long> { static const int value = 2; };

        template<> struct arithmetic_grade<int> { static const int value = 1; };

        template<> struct arithmetic_grade<unsigned int> { static const int value = 1; };
        template<typename L, typename R>

        struct binary_promote : public

            boost::enable_if<boost::mpl::and_<boost::is_arithmetic<L>,

                                              boost::is_arithmetic<R> >,

                             typename boost::promote<

                                 typename boost::mpl::if_c<

                                     (arithmetic_grade<L>::value >= arithmetic_grade<R>::value),

                                     L,

                                     R>::type>::type>

        {};

    } // namespace detail


    template<class Vec, typename Elem>

    struct vector_base {

        typedef Vec self_type;

        typedef Elem element_type;
        // v1 += v2

        template<typename Cont>

        self_type& operator+=(Cont const & v) {

            self_type & self = static_cast<self_type &>(*this);

            for (size_t i = 0; i < 3; ++i) {

                self[i] += v[i];

            }

            return self;

        }
        // v1 -= v2

        template<typename Cont>

        self_type& operator-=(Cont const & v) {

            self_type & self = static_cast<self_type &>(*this);

            for (size_t i = 0; i < 3; ++i) {

                self[i] -= v[i];

            }

            return self;

        }
    protected:

        // v1 = v2

        template<typename Cont>

        self_type & assign(Cont const & v) {

            self_type & self = static_cast<self_type &>(*this);

            for (size_t i = 0; i < 3; ++i) {

                self[i] = v[i];

            }

            return self;

        }

        self_type & assign(zero_vector_type) {

            self_type & self = static_cast<self_type &>(*this);

            for (size_t i = 0; i < 4; ++i) {

                self[i] = element_type(0);

            }

            return self;

        }
    public:

        // ||v||

        element_type norm() const {

            self_type const & self = static_cast<self_type const &>(*this);

            return std::sqrt(self[0] * self[0] + self[1] * self[1] + self[2] * self[2]);

        }
        // v = (x y z 0)

        self_type & assign_element(element_type x, element_type y, element_type z) {

            self_type & self = static_cast<self_type &>(*this);

            self[0] = x;

            self[1] = y;

            self[2] = z;

            return static_cast<self_type &>(*this);

        }
        // v = (x y z w)

        self_type & assign_element(element_type x, element_type y, element_type z, element_type w) {

            self_type & self = static_cast<self_type &>(*this);

            assign_element(x, y, z);

            self[3] = w;

            return self;

        }
        // duplicate

        vector<element_type> dup() const {

            return vector<element_type>(static_cast<self_type const &>(*this));

        }

    };
    // vector

    template<typename Elem>

    struct vector : public vector_base<vector<Elem>, Elem> {

        typedef vector_base<vector<Elem>, Elem> base_type;

        typedef typename base_type::element_type element_type;
        element_type raw_[4];
        vector(element_type const & x, element_type const & y, element_type const & z) {

            this->raw_[0] = x; this->raw_[1] = y; this->raw_[2] = z; this->raw_[3] = element_type(0);

        }
        vector(element_type const & x, element_type const & y, element_type const & z, element_type const & w) {

            this->raw_[0] = x; this->raw_[1] = y; this->raw_[2] = z; this->raw_[3] = w;

        }
        template<typename Cont>

        vector(Cont const & v) {

            this->assign(v);

        }
        vector() {}
        // v[i]

        element_type& operator[](size_t i) {

            return raw_[i];

        }
        element_type const & operator[](size_t i) const {

            return raw_[i];

        }
        template<typename Cont>

        vector& operator=(Cont const & v) {

            return this->assign(v);

        }

    };
    // adaptor vector

    template<typename Elem, class Cont>

    struct adaptor_vector : public vector_base<adaptor_vector<Elem, Cont>, Elem> {

        Cont const & container_;

        typedef Elem element_type;
        adaptor_vector(Cont const & container) : container_(container) {}

        element_type operator[](size_t i) const {

            return container_[i];

        }

    };
    template<typename Elem, class Cont>

    adaptor_vector<Elem, Cont> make_adaptor_vector(Cont const & cont) {

        return adaptor_vector<Elem, Cont>(cont);

    }
    // -v

    template<typename Vec>

    struct negative_vector : public vector_base<Vec, typename Vec::element_type> {

        Vec const & vec_;

        typedef typename Vec::element_type element_type;
        negative_vector(Vec const & vec) : vec_(vec) {}

        element_type operator[](size_t i) const {

            return -vec_[i];

        }

    };
    template<typename Vec>

    negative_vector<Vec> operator-(Vec const & vec) {

        return negative_vector<Vec>(vec);

    }
    // vector operation

    template<typename Expr, typename LVec, typename RVec>

    struct vector_binary_op : public

        vector_base<Expr,

                    typename detail::binary_promote<typename LVec::element_type,

                                                    typename RVec::element_type>::type>

    {

        typedef LVec lhs_type;

        typedef RVec rhs_type;

        typedef typename lhs_type::element_type lhs_element_type;

        typedef typename rhs_type::element_type rhs_element_type;

        typedef typename detail::binary_promote<lhs_element_type, rhs_element_type>::type element_type;
        lhs_type const & lhs_;

        rhs_type const & rhs_;
        vector_binary_op(lhs_type const & lhs, rhs_type const & rhs) : lhs_(lhs), rhs_(rhs) {}

    };
    // v1 + v2

    template<typename LVec, typename RVec>

    struct vector_vector_add : public vector_binary_op<vector_vector_add<LVec, RVec>, LVec, RVec> {

        typedef vector_binary_op<vector_vector_add<LVec, RVec>, LVec, RVec> base_type;

        typedef typename base_type::element_type element_type;

        typedef typename base_type::lhs_type lhs_type;

        typedef typename base_type::rhs_type rhs_type;
        vector_vector_add(lhs_type const & lhs, rhs_type const & rhs) : base_type(lhs, rhs) {}
        element_type operator[](size_t i) const {

            return this->lhs_[i] + this->rhs_[i];

        }

    };
    template<typename LVec, typename LElem, typename RVec, typename RElem>

    vector_vector_add<LVec, RVec> const operator+(vector_base<LVec, LElem> const & v1, vector_base<RVec, RElem> const & v2) {

        return vector_vector_add<LVec, RVec>(static_cast<LVec const &>(v1), static_cast<RVec const &>(v2));

    }
    // v1 - v2

    template<typename LVec, typename RVec>

    struct vector_vector_sub : vector_binary_op<vector_vector_sub<LVec, RVec>, LVec, RVec> {

        typedef vector_binary_op<vector_vector_sub<LVec, RVec>, LVec, RVec> base_type;

        typedef typename base_type::element_type element_type;

        typedef typename base_type::lhs_type lhs_type;

        typedef typename base_type::rhs_type rhs_type;

        vector_vector_sub(lhs_type const & lhs, rhs_type const & rhs) : base_type(lhs, rhs) {}
        element_type operator[](size_t i) const {

            return this->lhs_[i] - this->rhs_[i];

        }

    };
    template<typename LVec, typename LElem, typename RVec, typename RElem>

    vector_vector_sub<LVec, RVec> const operator-(vector_base<LVec, LElem> const & v1, vector_base<RVec, RElem> const & v2) {

        return vector_vector_sub<LVec, RVec>(static_cast<LVec const &>(v1), static_cast<RVec const &>(v2));

    }


    // v1 * v2

    template<typename LVec, typename RVec>

    struct vector_vector_mul : public vector_binary_op<vector_vector_mul<LVec, RVec>, LVec, RVec> {

        typedef vector_binary_op<vector_vector_mul<LVec, RVec>, LVec, RVec> base_type;

        typedef typename base_type::element_type element_type;

        typedef typename base_type::lhs_type lhs_type;

        typedef typename base_type::rhs_type rhs_type;

        vector_vector_mul(lhs_type const & lhs, rhs_type const & rhs) : base_type(lhs, rhs) {}
        element_type operator[](size_t i) const {

            return this->lhs_[i] * this->rhs_[i];

        }

    };
    template<typename LVec, typename LElem, typename RVec, typename RElem>

    vector_vector_mul<LVec, RVec> const operator*(vector_base<LVec, LElem> const & v1, vector_base<RVec, RElem> const & v2) {

        return vector_vector_mul<LVec, RVec>(static_cast<LVec const &>(v1), static_cast<RVec const &>(v2));

    }
    // v * s

    template<typename Vec, typename Sca>

    struct vector_scale : public

        vector_base<vector_scale<Vec, Sca>,

                    typename detail::binary_promote<typename Vec::element_type, Sca>::type>

    {

        typedef Vec vector_type;

        typedef Sca scalar_type;

        typedef typename detail::binary_promote<typename Vec::element_type, Sca>::type element_type;
        vector_type const & v_;

        scalar_type const & s_;
        vector_scale(vector_type const & v, scalar_type const & s) : v_(v), s_(s) {}
        element_type operator[](size_t i) const {

            return v_[i] * s_;

        }

    };
    template<typename Vec, typename Scalar>

    typename boost::disable_if<boost::mpl::or_<detail::is_vector<Scalar>, detail::is_matrix<Scalar> >,

                               vector_scale<Vec, Scalar> const>::type

    operator*(vector_base<Vec, typename Vec::element_type> const & v, Scalar const & s) {

        return vector_scale<Vec, Scalar>(static_cast<Vec const &>(v), s);

    }
    // cross_prod

    template<typename LVec, typename RVec>

    struct vector_cross_prod : public vector_binary_op<vector_cross_prod<LVec, RVec>, LVec, RVec> {

        typedef vector_binary_op<vector_cross_prod<LVec, RVec>, LVec, RVec> base_type;

        typedef typename base_type::element_type element_type;

        typedef typename base_type::lhs_type lhs_type;

        typedef typename base_type::rhs_type rhs_type;

        using base_type::lhs_;

        using base_type::rhs_;
        vector_cross_prod(lhs_type const & lhs, rhs_type const & rhs) : base_type(lhs, rhs) {}

        element_type operator[](size_t i) const {

            if (i == 3) { return element_type(0); }

            size_t const index1 = i == 2 ? 0 : i + 1;

            size_t const index2 = !i ? 2 : i - 1;

            return lhs_[index1] * rhs_[index2] - lhs_[index2] * rhs_[index1];

        }

    };
    template<typename LVec, typename RVec>

    vector_cross_prod<LVec, RVec> cross_prod(LVec const & v1, RVec const & v2) {

        return vector_cross_prod<LVec, RVec>(v1, v2);

    }
    // prod

    template<typename Vec, typename Mat>

    struct vector_matrix_prod : public vector_binary_op<vector_matrix_prod<Vec, Mat>, Vec, Mat> {

        typedef vector_binary_op<vector_matrix_prod<Vec, Mat>, Vec, Mat> base_type;

        typedef typename base_type::element_type element_type;

        typedef typename base_type::lhs_type lhs_type;

        typedef typename base_type::rhs_type rhs_type;

        using base_type::lhs_;

        using base_type::rhs_;
        vector_matrix_prod(Vec const & v, Mat const & m) : base_type(v, m) {}

        element_type operator[](size_t i) const {

            return lhs_[0] * rhs_[(0 << 2) + i]

                 + lhs_[1] * rhs_[(1 << 2) + i]

                 + lhs_[2] * rhs_[(2 << 2) + i]

                 + rhs_[(3 << 2) + i];

        }

    };
    template<typename Vec, typename Mat>

    typename boost::enable_if<

        boost::mpl::and_<detail::is_vector<Vec>, detail::is_matrix<Mat> >,

        vector_matrix_prod<Vec, Mat> >::type

    operator*(Vec const & v, Mat const & m) {

        return vector_matrix_prod<Vec, Mat>(v, m);

    }
    // rotate

    template<typename Vec, typename Mat>

    struct vector_rotate : public vector_binary_op<vector_rotate<Vec, Mat>, Vec, Mat> {

        typedef vector_binary_op<vector_rotate<Vec, Mat>, Vec, Mat> base_type;

        typedef typename base_type::element_type element_type;

        typedef typename base_type::lhs_type lhs_type;

        typedef typename base_type::rhs_type rhs_type;

        using base_type::lhs_;

        using base_type::rhs_;
        vector_rotate(Vec const & v, Mat const & m) : base_type(v, m) {}

        element_type operator[](size_t i) const {

            return lhs_[0] * rhs_[(i << 2) + 0]

                 + lhs_[1] * rhs_[(i << 2) + 1]

                 + lhs_[2] * rhs_[(i << 2) + 2];

        }

    };
    template<typename Vec, typename Mat>

    vector_rotate<Vec, Mat> rotate(Vec const & v, Mat const & m) {

        return vector_rotate<Vec, Mat>(v, m);

    }
    // dot prod(v1, v2)

    template<typename LVec, typename RVec>

    typename detail::binary_promote<typename LVec::element_type, typename RVec::element_type>::type

    dot_prod(vector_base<LVec, typename LVec::element_type> const & v1, vector_base<RVec, typename RVec::element_type> const & v2) {

        typedef typename detail::binary_promote<typename LVec::element_type, typename RVec::element_type>::type result_type;

        result_type ret = result_type(0);

        for (size_t i = 0; i < 3; i++) {

            ret += static_cast<LVec const &>(v1)[i] * static_cast<RVec const &>(v2)[i];

        }

        return ret;

    }
    template<typename Mat, typename Elem>

    struct matrix_base {
        typedef Mat self_type;

        typedef Elem element_type;
        // m1 += m2

        template<typename Cont>

        self_type & operator+=(Cont const & m) {

            self_type & self = static_cast<self_type &>(*this);

            for (size_t i = 0; i < 16; ++i) {

                self[i] += m[i];

            }

            return self;

        }
        // m1 -= m2

        template<typename Cont>

        self_type& operator-=(Cont const & m) {

            self_type & self = static_cast<self_type &>(*this);

            for (size_t i = 0; i < 16; ++i) {

                self[i] -= m[i];

            }

            return self;

        }
    protected:

        // m1 = m2

        template<typename Cont>

        self_type & assign(Cont const & m) {

            self_type & self = static_cast<self_type &>(*this);

            for (size_t i = 0; i < 16; ++i) {

                self[i] = m[i];

            }

        }
        // m = identity

        self_type & assign(identity_type) {

            self_type & self = static_cast<self_type &>(*this);

            for (size_t i = 0; i < 16; ++i) {

                self[i] = element_type)((i >> 2) == (i & 0x03))(;

            }

            return self;

        }
        // m = zero_matrix

        self_type & assign(zero_matrix_type) {

            self_type & self = static_cast<self_type &>(*this);

            for (size_t i = 0; i < 16; ++i) {

                self[i] = element_type(0);

            }

            return self;

        }
    public:

        // m.row(i)

        matrix_row<self_type> row(size_t i) {

            return matrix_row<self_type>(static_cast<self_type &>(*this), i);

        }

        matrix_row<self_type const> const row(size_t i) const {

            return matrix_row<self_type const>(static_cast<self_type const &>(*this), i);

        }
        // m.col(i)

        matrix_column<self_type> col(size_t i) {

            return matrix_column<self_type>(static_cast<self_type &>(*this), i);

        }

        matrix_column<self_type const> const col(size_t i) const {

            return matrix_column<self_type const>(static_cast<self_type const &>(*this), i);

        }
        // duplicate

        matrix<Elem> dup() const {

            return static_cast<self_type const &>(*this);

        }

    };
    template<typename Elem>

    struct matrix : public matrix_base<matrix<Elem>, Elem> {

        typedef matrix_base<matrix<Elem>, Elem> base_type;

        typedef typename base_type::element_type element_type;
        union {

            element_type raw_[16];

        };


        matrix() {}
        template<typename Cont>

        matrix(Cont const & m) {

            this->assign(m);

        }

        template<typename Cont>

        matrix& operator=(Cont const & m) {

            return this->assign(m);

        }
        // m[i]

        element_type & operator[](size_t i) {

            return raw_[i];

        }

        element_type const & operator[](size_t i) const {

            return raw_[i];

        }
        // m(i, j)

        element_type & operator()(size_t i, size_t j) {

            return raw_[(i << 2) + j];

        }

        element_type const & operator()(size_t i, size_t j) const {

            return raw_[(i << 2) + j];

        }

    };
    // matrix row

    template<typename Mat>

    class matrix_row : public vector_base<matrix_row<Mat>, typename Mat::element_type> {

    public:

        typedef Mat matrix_type;

        typedef typename matrix_type::element_type element_type;
    private:

        matrix_type const & mat_;

        size_t const row_;
    public:

        matrix_row(matrix_type const & mat, size_t row) : mat_(mat), row_(row) {

            if (row_ >= 4) {

                throw std::invalid_argument("row is out of range");

            }

        }
        element_type operator[](size_t i) const {

            return mat_[(row_ << 2) + i];

        }

    };
    template<typename Elem>

    class matrix_row<matrix<Elem> > : public vector_base<matrix_row<matrix<Elem> >, typename matrix<Elem>::element_type> {

    public:

        typedef matrix<Elem> matrix_type;

        typedef typename matrix_type::element_type element_type;
    private:

        matrix_type & mat_;

        size_t const row_;
    public:

        matrix_row(matrix_type & mat, size_t row) : mat_(mat), row_(row) {

            if (row_ >= 4) {

                throw std::invalid_argument("row is out of range");

            }

        }
        element_type & operator[](size_t i) {

            return mat_[(row_ << 2) + i];

        }

        element_type const & operator[](size_t i) const {

            return mat_[(row_ << 2) + i];

        }
        template<typename Cont>

        matrix_row& operator=(Cont const & v) {

            this->assign(v);

            return *this;

        }

    };
    // matrix column

    template<typename Mat>

    class matrix_column : public vector_base<matrix_column<Mat>, typename Mat::element_type> {

    public:

        typedef Mat matrix_type;

        typedef typename Mat::element_type element_type;
    private:

        matrix_type const & mat_;

        size_t const col_;
    public:

        matrix_column(matrix_type const & mat, size_t col) : mat_(mat), col_(col) {

            if (col_ >= 4) {

                throw std::invalid_argument("column is out of range");

            }

        }
        element_type operator[](size_t i) const {

            return mat_[(i << 2) + col_];

        }

    };
    template<typename Elem>

    class matrix_column<matrix<Elem> > : public vector_base<matrix_column<matrix<Elem> >, typename matrix<Elem>::element_type> {

    public:

        typedef matrix<Elem> matrix_type;

        typedef typename matrix_type::element_type element_type;
    private:

        matrix_type & mat_;

        size_t const col_;
    public:

        matrix_column(matrix_type& mat, size_t col) : mat_(mat), col_(col) {

            if (col_ >= 4) {

                throw std::invalid_argument("column is out of range");

            }

        }
        element_type & operator[](size_t i) {

            return mat_[(i << 2) + col_];

        }

        element_type const & operator[](size_t i) const {

            return mat_[(i << 2) + col_];

        }
        template<typename Cont>

        matrix_column & operator=(Cont const & v) {

            this->assign(v);

            return *this;

        }

    };
    // transpose matrix

    template<typename Mat>

    class transposed_matrix : public matrix_base<transposed_matrix<Mat>, typename Mat::element_type> {

        Mat const & mat_;

    public:

        typedef typename Mat::element_type element_type;
        element_type operator[](size_t i) const {

            return mat_[(i << 2) & 0x0c | (i >> 2)];

        }
        transposed_matrix(Mat const & mat) : mat_(mat) {}

    };
    template<typename Mat, typename Elem>

    transposed_matrix<Mat> transpose(matrix_base<Mat, Elem> const & m) {

        return transposed_matrix<Mat>(static_cast<Mat const &>(m));

    }
    template<typename Expr, typename Lhs, typename Rhs>

    struct matrix_binary_op : public

        matrix_base<Expr,

                    typename detail::binary_promote<typename Lhs::element_type,

                                                    typename Rhs::element_type>::type>

    {

        typedef Lhs lhs_type;

        typedef Rhs rhs_type;

        typedef typename lhs_type::element_type lhs_element_type;

        typedef typename rhs_type::element_type rhs_element_type;

        typedef typename detail::binary_promote<lhs_element_type, rhs_element_type>::type element_type;

    protected:

        lhs_type const & lhs_;

        rhs_type const & rhs_;

        matrix_binary_op(lhs_type const & lhs, rhs_type const & rhs) : lhs_(lhs), rhs_(rhs) {}

    };
    template<typename LMat, typename RMat>

    struct matrix_matrix_mul : public matrix_binary_op<matrix_matrix_mul<LMat, RMat>, LMat, RMat> {

        typedef matrix_binary_op<matrix_matrix_mul<LMat, RMat>, LMat, RMat> base_type;

        typedef typename base_type::lhs_type lhs_type;

        typedef typename base_type::rhs_type rhs_type;

        typedef typename base_type::element_type element_type;

        using base_type::lhs_;

        using base_type::rhs_;
        matrix_matrix_mul(lhs_type const & lhs, rhs_type const & rhs) : base_type(lhs, rhs) {}

        element_type operator[](size_t i) const {

            size_t const rowIndex = i >> 2;

            size_t const colIndex = i & 0x03;

            return dot_prod(this->lhs_.row(rowIndex), this->rhs_.col(colIndex)) + this->lhs_[(i & 0x0c) + 3] * this->rhs_[(3 << 2) + (i & 0x03)];

        }

    };
    template<typename LMat, typename RMat>

    typename boost::enable_if<boost::mpl::and_<detail::is_matrix<LMat>,

                                               detail::is_matrix<RMat> >,

                              matrix_matrix_mul<LMat, RMat> >::type

    operator*(LMat const & m1, RMat const & m2) {

        return matrix_matrix_mul<LMat, RMat>(m1, m2);

    }
}
#endif    // NEWTYPE_HPP_INCLUDED_