#ifndef BOOST_QVM_GEN_MAT_OPERATIONS4_HPP_INCLUDED
#define BOOST_QVM_GEN_MAT_OPERATIONS4_HPP_INCLUDED

// Copyright 2008-2022 Emil Dotchevski and Reverge Studios, Inc.

// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// This file was generated by a program. Do not edit manually.

#include <boost/qvm/assert.hpp>
#include <boost/qvm/deduce_mat.hpp>
#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/error.hpp>
#include <boost/qvm/gen/mat_assign4.hpp>
#include <boost/qvm/quat_traits.hpp>
#include <boost/qvm/scalar_traits.hpp>
#include <boost/qvm/throw_exception.hpp>

namespace boost { namespace qvm {

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    deduce_mat2<A,B,4,4> >::type
operator+( A const & a, B const & b )
    {
    typedef typename deduce_mat2<A,B,4,4>::type R;
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==4);
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==4);
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)+mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(r,mat_traits<A>::template read_element<0,1>(a)+mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(r,mat_traits<A>::template read_element<0,2>(a)+mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(r,mat_traits<A>::template read_element<0,3>(a)+mat_traits<B>::template read_element<0,3>(b));
    write_mat_element<1,0>(r,mat_traits<A>::template read_element<1,0>(a)+mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<1,1>(r,mat_traits<A>::template read_element<1,1>(a)+mat_traits<B>::template read_element<1,1>(b));
    write_mat_element<1,2>(r,mat_traits<A>::template read_element<1,2>(a)+mat_traits<B>::template read_element<1,2>(b));
    write_mat_element<1,3>(r,mat_traits<A>::template read_element<1,3>(a)+mat_traits<B>::template read_element<1,3>(b));
    write_mat_element<2,0>(r,mat_traits<A>::template read_element<2,0>(a)+mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<2,1>(r,mat_traits<A>::template read_element<2,1>(a)+mat_traits<B>::template read_element<2,1>(b));
    write_mat_element<2,2>(r,mat_traits<A>::template read_element<2,2>(a)+mat_traits<B>::template read_element<2,2>(b));
    write_mat_element<2,3>(r,mat_traits<A>::template read_element<2,3>(a)+mat_traits<B>::template read_element<2,3>(b));
    write_mat_element<3,0>(r,mat_traits<A>::template read_element<3,0>(a)+mat_traits<B>::template read_element<3,0>(b));
    write_mat_element<3,1>(r,mat_traits<A>::template read_element<3,1>(a)+mat_traits<B>::template read_element<3,1>(b));
    write_mat_element<3,2>(r,mat_traits<A>::template read_element<3,2>(a)+mat_traits<B>::template read_element<3,2>(b));
    write_mat_element<3,3>(r,mat_traits<A>::template read_element<3,3>(a)+mat_traits<B>::template read_element<3,3>(b));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator+;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct plus_mm_defined;

    template <>
    struct
    plus_mm_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
    deduce_mat2<A,B,4,1> >::type
operator+( A const & a, B const & b )
    {
    typedef typename deduce_mat2<A,B,4,1>::type R;
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==4);
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==1);
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)+mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<1,0>(r,mat_traits<A>::template read_element<1,0>(a)+mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<2,0>(r,mat_traits<A>::template read_element<2,0>(a)+mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<3,0>(r,mat_traits<A>::template read_element<3,0>(a)+mat_traits<B>::template read_element<3,0>(b));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator+;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct plus_mm_defined;

    template <>
    struct
    plus_mm_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    deduce_mat2<A,B,1,4> >::type
operator+( A const & a, B const & b )
    {
    typedef typename deduce_mat2<A,B,1,4>::type R;
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==1);
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==4);
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)+mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(r,mat_traits<A>::template read_element<0,1>(a)+mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(r,mat_traits<A>::template read_element<0,2>(a)+mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(r,mat_traits<A>::template read_element<0,3>(a)+mat_traits<B>::template read_element<0,3>(b));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator+;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct plus_mm_defined;

    template <>
    struct
    plus_mm_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    deduce_mat2<A,B,4,4> >::type
operator-( A const & a, B const & b )
    {
    typedef typename deduce_mat2<A,B,4,4>::type R;
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==4);
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==4);
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)-mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(r,mat_traits<A>::template read_element<0,1>(a)-mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(r,mat_traits<A>::template read_element<0,2>(a)-mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(r,mat_traits<A>::template read_element<0,3>(a)-mat_traits<B>::template read_element<0,3>(b));
    write_mat_element<1,0>(r,mat_traits<A>::template read_element<1,0>(a)-mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<1,1>(r,mat_traits<A>::template read_element<1,1>(a)-mat_traits<B>::template read_element<1,1>(b));
    write_mat_element<1,2>(r,mat_traits<A>::template read_element<1,2>(a)-mat_traits<B>::template read_element<1,2>(b));
    write_mat_element<1,3>(r,mat_traits<A>::template read_element<1,3>(a)-mat_traits<B>::template read_element<1,3>(b));
    write_mat_element<2,0>(r,mat_traits<A>::template read_element<2,0>(a)-mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<2,1>(r,mat_traits<A>::template read_element<2,1>(a)-mat_traits<B>::template read_element<2,1>(b));
    write_mat_element<2,2>(r,mat_traits<A>::template read_element<2,2>(a)-mat_traits<B>::template read_element<2,2>(b));
    write_mat_element<2,3>(r,mat_traits<A>::template read_element<2,3>(a)-mat_traits<B>::template read_element<2,3>(b));
    write_mat_element<3,0>(r,mat_traits<A>::template read_element<3,0>(a)-mat_traits<B>::template read_element<3,0>(b));
    write_mat_element<3,1>(r,mat_traits<A>::template read_element<3,1>(a)-mat_traits<B>::template read_element<3,1>(b));
    write_mat_element<3,2>(r,mat_traits<A>::template read_element<3,2>(a)-mat_traits<B>::template read_element<3,2>(b));
    write_mat_element<3,3>(r,mat_traits<A>::template read_element<3,3>(a)-mat_traits<B>::template read_element<3,3>(b));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct minus_mm_defined;

    template <>
    struct
    minus_mm_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
    deduce_mat2<A,B,4,1> >::type
operator-( A const & a, B const & b )
    {
    typedef typename deduce_mat2<A,B,4,1>::type R;
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==4);
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==1);
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)-mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<1,0>(r,mat_traits<A>::template read_element<1,0>(a)-mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<2,0>(r,mat_traits<A>::template read_element<2,0>(a)-mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<3,0>(r,mat_traits<A>::template read_element<3,0>(a)-mat_traits<B>::template read_element<3,0>(b));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct minus_mm_defined;

    template <>
    struct
    minus_mm_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    deduce_mat2<A,B,1,4> >::type
operator-( A const & a, B const & b )
    {
    typedef typename deduce_mat2<A,B,1,4>::type R;
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==1);
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==4);
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)-mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(r,mat_traits<A>::template read_element<0,1>(a)-mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(r,mat_traits<A>::template read_element<0,2>(a)-mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(r,mat_traits<A>::template read_element<0,3>(a)-mat_traits<B>::template read_element<0,3>(b));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct minus_mm_defined;

    template <>
    struct
    minus_mm_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    A &>::type
operator+=( A & a, B const & b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)+mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(a,mat_traits<A>::template read_element<0,1>(a)+mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(a,mat_traits<A>::template read_element<0,2>(a)+mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(a,mat_traits<A>::template read_element<0,3>(a)+mat_traits<B>::template read_element<0,3>(b));
    write_mat_element<1,0>(a,mat_traits<A>::template read_element<1,0>(a)+mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<1,1>(a,mat_traits<A>::template read_element<1,1>(a)+mat_traits<B>::template read_element<1,1>(b));
    write_mat_element<1,2>(a,mat_traits<A>::template read_element<1,2>(a)+mat_traits<B>::template read_element<1,2>(b));
    write_mat_element<1,3>(a,mat_traits<A>::template read_element<1,3>(a)+mat_traits<B>::template read_element<1,3>(b));
    write_mat_element<2,0>(a,mat_traits<A>::template read_element<2,0>(a)+mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<2,1>(a,mat_traits<A>::template read_element<2,1>(a)+mat_traits<B>::template read_element<2,1>(b));
    write_mat_element<2,2>(a,mat_traits<A>::template read_element<2,2>(a)+mat_traits<B>::template read_element<2,2>(b));
    write_mat_element<2,3>(a,mat_traits<A>::template read_element<2,3>(a)+mat_traits<B>::template read_element<2,3>(b));
    write_mat_element<3,0>(a,mat_traits<A>::template read_element<3,0>(a)+mat_traits<B>::template read_element<3,0>(b));
    write_mat_element<3,1>(a,mat_traits<A>::template read_element<3,1>(a)+mat_traits<B>::template read_element<3,1>(b));
    write_mat_element<3,2>(a,mat_traits<A>::template read_element<3,2>(a)+mat_traits<B>::template read_element<3,2>(b));
    write_mat_element<3,3>(a,mat_traits<A>::template read_element<3,3>(a)+mat_traits<B>::template read_element<3,3>(b));
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator+=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct plus_eq_mm_defined;

    template <>
    struct
    plus_eq_mm_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
    A &>::type
operator+=( A & a, B const & b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)+mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<1,0>(a,mat_traits<A>::template read_element<1,0>(a)+mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<2,0>(a,mat_traits<A>::template read_element<2,0>(a)+mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<3,0>(a,mat_traits<A>::template read_element<3,0>(a)+mat_traits<B>::template read_element<3,0>(b));
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator+=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct plus_eq_mm_defined;

    template <>
    struct
    plus_eq_mm_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    A &>::type
operator+=( A & a, B const & b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)+mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(a,mat_traits<A>::template read_element<0,1>(a)+mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(a,mat_traits<A>::template read_element<0,2>(a)+mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(a,mat_traits<A>::template read_element<0,3>(a)+mat_traits<B>::template read_element<0,3>(b));
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator+=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct plus_eq_mm_defined;

    template <>
    struct
    plus_eq_mm_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    A &>::type
operator-=( A & a, B const & b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)-mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(a,mat_traits<A>::template read_element<0,1>(a)-mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(a,mat_traits<A>::template read_element<0,2>(a)-mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(a,mat_traits<A>::template read_element<0,3>(a)-mat_traits<B>::template read_element<0,3>(b));
    write_mat_element<1,0>(a,mat_traits<A>::template read_element<1,0>(a)-mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<1,1>(a,mat_traits<A>::template read_element<1,1>(a)-mat_traits<B>::template read_element<1,1>(b));
    write_mat_element<1,2>(a,mat_traits<A>::template read_element<1,2>(a)-mat_traits<B>::template read_element<1,2>(b));
    write_mat_element<1,3>(a,mat_traits<A>::template read_element<1,3>(a)-mat_traits<B>::template read_element<1,3>(b));
    write_mat_element<2,0>(a,mat_traits<A>::template read_element<2,0>(a)-mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<2,1>(a,mat_traits<A>::template read_element<2,1>(a)-mat_traits<B>::template read_element<2,1>(b));
    write_mat_element<2,2>(a,mat_traits<A>::template read_element<2,2>(a)-mat_traits<B>::template read_element<2,2>(b));
    write_mat_element<2,3>(a,mat_traits<A>::template read_element<2,3>(a)-mat_traits<B>::template read_element<2,3>(b));
    write_mat_element<3,0>(a,mat_traits<A>::template read_element<3,0>(a)-mat_traits<B>::template read_element<3,0>(b));
    write_mat_element<3,1>(a,mat_traits<A>::template read_element<3,1>(a)-mat_traits<B>::template read_element<3,1>(b));
    write_mat_element<3,2>(a,mat_traits<A>::template read_element<3,2>(a)-mat_traits<B>::template read_element<3,2>(b));
    write_mat_element<3,3>(a,mat_traits<A>::template read_element<3,3>(a)-mat_traits<B>::template read_element<3,3>(b));
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct minus_eq_mm_defined;

    template <>
    struct
    minus_eq_mm_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
    A &>::type
operator-=( A & a, B const & b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)-mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<1,0>(a,mat_traits<A>::template read_element<1,0>(a)-mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<2,0>(a,mat_traits<A>::template read_element<2,0>(a)-mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<3,0>(a,mat_traits<A>::template read_element<3,0>(a)-mat_traits<B>::template read_element<3,0>(b));
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct minus_eq_mm_defined;

    template <>
    struct
    minus_eq_mm_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    A &>::type
operator-=( A & a, B const & b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)-mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(a,mat_traits<A>::template read_element<0,1>(a)-mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(a,mat_traits<A>::template read_element<0,2>(a)-mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(a,mat_traits<A>::template read_element<0,3>(a)-mat_traits<B>::template read_element<0,3>(b));
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct minus_eq_mm_defined;

    template <>
    struct
    minus_eq_mm_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==4 && is_scalar<B>::value,
    deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols> >::type
operator*( A const & a, B b )
    {
    typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols>::type R;
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)*b);
    write_mat_element<0,1>(r,mat_traits<A>::template read_element<0,1>(a)*b);
    write_mat_element<0,2>(r,mat_traits<A>::template read_element<0,2>(a)*b);
    write_mat_element<0,3>(r,mat_traits<A>::template read_element<0,3>(a)*b);
    write_mat_element<1,0>(r,mat_traits<A>::template read_element<1,0>(a)*b);
    write_mat_element<1,1>(r,mat_traits<A>::template read_element<1,1>(a)*b);
    write_mat_element<1,2>(r,mat_traits<A>::template read_element<1,2>(a)*b);
    write_mat_element<1,3>(r,mat_traits<A>::template read_element<1,3>(a)*b);
    write_mat_element<2,0>(r,mat_traits<A>::template read_element<2,0>(a)*b);
    write_mat_element<2,1>(r,mat_traits<A>::template read_element<2,1>(a)*b);
    write_mat_element<2,2>(r,mat_traits<A>::template read_element<2,2>(a)*b);
    write_mat_element<2,3>(r,mat_traits<A>::template read_element<2,3>(a)*b);
    write_mat_element<3,0>(r,mat_traits<A>::template read_element<3,0>(a)*b);
    write_mat_element<3,1>(r,mat_traits<A>::template read_element<3,1>(a)*b);
    write_mat_element<3,2>(r,mat_traits<A>::template read_element<3,2>(a)*b);
    write_mat_element<3,3>(r,mat_traits<A>::template read_element<3,3>(a)*b);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct mul_ms_defined;

    template <>
    struct
    mul_ms_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    is_scalar<A>::value && mat_traits<B>::rows==4 && mat_traits<B>::cols==4,
    deduce_mat2<A,B,mat_traits<B>::rows,mat_traits<B>::cols> >::type
operator*( A a, B const & b )
    {
    typedef typename deduce_mat2<A,B,mat_traits<B>::rows,mat_traits<B>::cols>::type R;
    R r;
    write_mat_element<0,0>(r,a*mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(r,a*mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(r,a*mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(r,a*mat_traits<B>::template read_element<0,3>(b));
    write_mat_element<1,0>(r,a*mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<1,1>(r,a*mat_traits<B>::template read_element<1,1>(b));
    write_mat_element<1,2>(r,a*mat_traits<B>::template read_element<1,2>(b));
    write_mat_element<1,3>(r,a*mat_traits<B>::template read_element<1,3>(b));
    write_mat_element<2,0>(r,a*mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<2,1>(r,a*mat_traits<B>::template read_element<2,1>(b));
    write_mat_element<2,2>(r,a*mat_traits<B>::template read_element<2,2>(b));
    write_mat_element<2,3>(r,a*mat_traits<B>::template read_element<2,3>(b));
    write_mat_element<3,0>(r,a*mat_traits<B>::template read_element<3,0>(b));
    write_mat_element<3,1>(r,a*mat_traits<B>::template read_element<3,1>(b));
    write_mat_element<3,2>(r,a*mat_traits<B>::template read_element<3,2>(b));
    write_mat_element<3,3>(r,a*mat_traits<B>::template read_element<3,3>(b));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct mul_sm_defined;

    template <>
    struct
    mul_sm_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==1 && is_scalar<B>::value,
    deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols> >::type
operator*( A const & a, B b )
    {
    typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols>::type R;
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)*b);
    write_mat_element<1,0>(r,mat_traits<A>::template read_element<1,0>(a)*b);
    write_mat_element<2,0>(r,mat_traits<A>::template read_element<2,0>(a)*b);
    write_mat_element<3,0>(r,mat_traits<A>::template read_element<3,0>(a)*b);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct mul_ms_defined;

    template <>
    struct
    mul_ms_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    is_scalar<A>::value && mat_traits<B>::rows==4 && mat_traits<B>::cols==1,
    deduce_mat2<A,B,mat_traits<B>::rows,mat_traits<B>::cols> >::type
operator*( A a, B const & b )
    {
    typedef typename deduce_mat2<A,B,mat_traits<B>::rows,mat_traits<B>::cols>::type R;
    R r;
    write_mat_element<0,0>(r,a*mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<1,0>(r,a*mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<2,0>(r,a*mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<3,0>(r,a*mat_traits<B>::template read_element<3,0>(b));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct mul_sm_defined;

    template <>
    struct
    mul_sm_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<A>::cols==4 && is_scalar<B>::value,
    deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols> >::type
operator*( A const & a, B b )
    {
    typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols>::type R;
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)*b);
    write_mat_element<0,1>(r,mat_traits<A>::template read_element<0,1>(a)*b);
    write_mat_element<0,2>(r,mat_traits<A>::template read_element<0,2>(a)*b);
    write_mat_element<0,3>(r,mat_traits<A>::template read_element<0,3>(a)*b);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct mul_ms_defined;

    template <>
    struct
    mul_ms_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    is_scalar<A>::value && mat_traits<B>::rows==1 && mat_traits<B>::cols==4,
    deduce_mat2<A,B,mat_traits<B>::rows,mat_traits<B>::cols> >::type
operator*( A a, B const & b )
    {
    typedef typename deduce_mat2<A,B,mat_traits<B>::rows,mat_traits<B>::cols>::type R;
    R r;
    write_mat_element<0,0>(r,a*mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(r,a*mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(r,a*mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(r,a*mat_traits<B>::template read_element<0,3>(b));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct mul_sm_defined;

    template <>
    struct
    mul_sm_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==4 && is_scalar<B>::value,
    A &>::type
operator*=( A & a, B b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)*b);
    write_mat_element<0,1>(a,mat_traits<A>::template read_element<0,1>(a)*b);
    write_mat_element<0,2>(a,mat_traits<A>::template read_element<0,2>(a)*b);
    write_mat_element<0,3>(a,mat_traits<A>::template read_element<0,3>(a)*b);
    write_mat_element<1,0>(a,mat_traits<A>::template read_element<1,0>(a)*b);
    write_mat_element<1,1>(a,mat_traits<A>::template read_element<1,1>(a)*b);
    write_mat_element<1,2>(a,mat_traits<A>::template read_element<1,2>(a)*b);
    write_mat_element<1,3>(a,mat_traits<A>::template read_element<1,3>(a)*b);
    write_mat_element<2,0>(a,mat_traits<A>::template read_element<2,0>(a)*b);
    write_mat_element<2,1>(a,mat_traits<A>::template read_element<2,1>(a)*b);
    write_mat_element<2,2>(a,mat_traits<A>::template read_element<2,2>(a)*b);
    write_mat_element<2,3>(a,mat_traits<A>::template read_element<2,3>(a)*b);
    write_mat_element<3,0>(a,mat_traits<A>::template read_element<3,0>(a)*b);
    write_mat_element<3,1>(a,mat_traits<A>::template read_element<3,1>(a)*b);
    write_mat_element<3,2>(a,mat_traits<A>::template read_element<3,2>(a)*b);
    write_mat_element<3,3>(a,mat_traits<A>::template read_element<3,3>(a)*b);
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct mul_eq_ms_defined;

    template <>
    struct
    mul_eq_ms_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==1 && is_scalar<B>::value,
    A &>::type
operator*=( A & a, B b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)*b);
    write_mat_element<1,0>(a,mat_traits<A>::template read_element<1,0>(a)*b);
    write_mat_element<2,0>(a,mat_traits<A>::template read_element<2,0>(a)*b);
    write_mat_element<3,0>(a,mat_traits<A>::template read_element<3,0>(a)*b);
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct mul_eq_ms_defined;

    template <>
    struct
    mul_eq_ms_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<A>::cols==4 && is_scalar<B>::value,
    A &>::type
operator*=( A & a, B b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)*b);
    write_mat_element<0,1>(a,mat_traits<A>::template read_element<0,1>(a)*b);
    write_mat_element<0,2>(a,mat_traits<A>::template read_element<0,2>(a)*b);
    write_mat_element<0,3>(a,mat_traits<A>::template read_element<0,3>(a)*b);
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct mul_eq_ms_defined;

    template <>
    struct
    mul_eq_ms_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==4 && is_scalar<B>::value,
    deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols> >::type
operator/( A const & a, B b )
    {
    typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols>::type R;
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)/b);
    write_mat_element<0,1>(r,mat_traits<A>::template read_element<0,1>(a)/b);
    write_mat_element<0,2>(r,mat_traits<A>::template read_element<0,2>(a)/b);
    write_mat_element<0,3>(r,mat_traits<A>::template read_element<0,3>(a)/b);
    write_mat_element<1,0>(r,mat_traits<A>::template read_element<1,0>(a)/b);
    write_mat_element<1,1>(r,mat_traits<A>::template read_element<1,1>(a)/b);
    write_mat_element<1,2>(r,mat_traits<A>::template read_element<1,2>(a)/b);
    write_mat_element<1,3>(r,mat_traits<A>::template read_element<1,3>(a)/b);
    write_mat_element<2,0>(r,mat_traits<A>::template read_element<2,0>(a)/b);
    write_mat_element<2,1>(r,mat_traits<A>::template read_element<2,1>(a)/b);
    write_mat_element<2,2>(r,mat_traits<A>::template read_element<2,2>(a)/b);
    write_mat_element<2,3>(r,mat_traits<A>::template read_element<2,3>(a)/b);
    write_mat_element<3,0>(r,mat_traits<A>::template read_element<3,0>(a)/b);
    write_mat_element<3,1>(r,mat_traits<A>::template read_element<3,1>(a)/b);
    write_mat_element<3,2>(r,mat_traits<A>::template read_element<3,2>(a)/b);
    write_mat_element<3,3>(r,mat_traits<A>::template read_element<3,3>(a)/b);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator/;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct div_ms_defined;

    template <>
    struct
    div_ms_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    is_scalar<A>::value && mat_traits<B>::rows==4 && mat_traits<B>::cols==4,
    deduce_mat2<A,B,mat_traits<B>::rows,mat_traits<B>::cols> >::type
operator/( A a, B const & b )
    {
    typedef typename deduce_mat2<A,B,mat_traits<B>::rows,mat_traits<B>::cols>::type R;
    R r;
    write_mat_element<0,0>(r,a/mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<0,1>(r,a/mat_traits<B>::template read_element<0,1>(b));
    write_mat_element<0,2>(r,a/mat_traits<B>::template read_element<0,2>(b));
    write_mat_element<0,3>(r,a/mat_traits<B>::template read_element<0,3>(b));
    write_mat_element<1,0>(r,a/mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<1,1>(r,a/mat_traits<B>::template read_element<1,1>(b));
    write_mat_element<1,2>(r,a/mat_traits<B>::template read_element<1,2>(b));
    write_mat_element<1,3>(r,a/mat_traits<B>::template read_element<1,3>(b));
    write_mat_element<2,0>(r,a/mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<2,1>(r,a/mat_traits<B>::template read_element<2,1>(b));
    write_mat_element<2,2>(r,a/mat_traits<B>::template read_element<2,2>(b));
    write_mat_element<2,3>(r,a/mat_traits<B>::template read_element<2,3>(b));
    write_mat_element<3,0>(r,a/mat_traits<B>::template read_element<3,0>(b));
    write_mat_element<3,1>(r,a/mat_traits<B>::template read_element<3,1>(b));
    write_mat_element<3,2>(r,a/mat_traits<B>::template read_element<3,2>(b));
    write_mat_element<3,3>(r,a/mat_traits<B>::template read_element<3,3>(b));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator/;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct div_sm_defined;

    template <>
    struct
    div_sm_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==1 && is_scalar<B>::value,
    deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols> >::type
operator/( A const & a, B b )
    {
    typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols>::type R;
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)/b);
    write_mat_element<1,0>(r,mat_traits<A>::template read_element<1,0>(a)/b);
    write_mat_element<2,0>(r,mat_traits<A>::template read_element<2,0>(a)/b);
    write_mat_element<3,0>(r,mat_traits<A>::template read_element<3,0>(a)/b);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator/;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct div_ms_defined;

    template <>
    struct
    div_ms_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    is_scalar<A>::value && mat_traits<B>::rows==4 && mat_traits<B>::cols==1,
    deduce_mat2<A,B,mat_traits<B>::rows,mat_traits<B>::cols> >::type
operator/( A a, B const & b )
    {
    typedef typename deduce_mat2<A,B,mat_traits<B>::rows,mat_traits<B>::cols>::type R;
    R r;
    write_mat_element<0,0>(r,a/mat_traits<B>::template read_element<0,0>(b));
    write_mat_element<1,0>(r,a/mat_traits<B>::template read_element<1,0>(b));
    write_mat_element<2,0>(r,a/mat_traits<B>::template read_element<2,0>(b));
    write_mat_element<3,0>(r,a/mat_traits<B>::template read_element<3,0>(b));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator/;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct div_sm_defined;

    template <>
    struct
    div_sm_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<A>::cols==4 && is_scalar<B>::value,
    deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols> >::type
operator/( A const & a, B b )
    {
    typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols>::type R;
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a)/b);
    write_mat_element<0,1>(r,mat_traits<A>::template read_element<0,1>(a)/b);
    write_mat_element<0,2>(r,mat_traits<A>::template read_element<0,2>(a)/b);
    write_mat_element<0,3>(r,mat_traits<A>::template read_element<0,3>(a)/b);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator/;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct div_ms_defined;

    template <>
    struct
    div_ms_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==4 && is_scalar<B>::value,
    A &>::type
operator/=( A & a, B b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)/b);
    write_mat_element<0,1>(a,mat_traits<A>::template read_element<0,1>(a)/b);
    write_mat_element<0,2>(a,mat_traits<A>::template read_element<0,2>(a)/b);
    write_mat_element<0,3>(a,mat_traits<A>::template read_element<0,3>(a)/b);
    write_mat_element<1,0>(a,mat_traits<A>::template read_element<1,0>(a)/b);
    write_mat_element<1,1>(a,mat_traits<A>::template read_element<1,1>(a)/b);
    write_mat_element<1,2>(a,mat_traits<A>::template read_element<1,2>(a)/b);
    write_mat_element<1,3>(a,mat_traits<A>::template read_element<1,3>(a)/b);
    write_mat_element<2,0>(a,mat_traits<A>::template read_element<2,0>(a)/b);
    write_mat_element<2,1>(a,mat_traits<A>::template read_element<2,1>(a)/b);
    write_mat_element<2,2>(a,mat_traits<A>::template read_element<2,2>(a)/b);
    write_mat_element<2,3>(a,mat_traits<A>::template read_element<2,3>(a)/b);
    write_mat_element<3,0>(a,mat_traits<A>::template read_element<3,0>(a)/b);
    write_mat_element<3,1>(a,mat_traits<A>::template read_element<3,1>(a)/b);
    write_mat_element<3,2>(a,mat_traits<A>::template read_element<3,2>(a)/b);
    write_mat_element<3,3>(a,mat_traits<A>::template read_element<3,3>(a)/b);
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator/=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct div_eq_ms_defined;

    template <>
    struct
    div_eq_ms_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==1 && is_scalar<B>::value,
    A &>::type
operator/=( A & a, B b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)/b);
    write_mat_element<1,0>(a,mat_traits<A>::template read_element<1,0>(a)/b);
    write_mat_element<2,0>(a,mat_traits<A>::template read_element<2,0>(a)/b);
    write_mat_element<3,0>(a,mat_traits<A>::template read_element<3,0>(a)/b);
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator/=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct div_eq_ms_defined;

    template <>
    struct
    div_eq_ms_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<A>::cols==4 && is_scalar<B>::value,
    A &>::type
operator/=( A & a, B b )
    {
    write_mat_element<0,0>(a,mat_traits<A>::template read_element<0,0>(a)/b);
    write_mat_element<0,1>(a,mat_traits<A>::template read_element<0,1>(a)/b);
    write_mat_element<0,2>(a,mat_traits<A>::template read_element<0,2>(a)/b);
    write_mat_element<0,3>(a,mat_traits<A>::template read_element<0,3>(a)/b);
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator/=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct div_eq_ms_defined;

    template <>
    struct
    div_eq_ms_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class R,class A>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<R>::rows==4 && mat_traits<A>::rows==4 &&
    mat_traits<R>::cols==4 && mat_traits<A>::cols==4,
    R>::type
convert_to( A const & a )
    {
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a));
    write_mat_element<0,1>(r,mat_traits<A>::template read_element<0,1>(a));
    write_mat_element<0,2>(r,mat_traits<A>::template read_element<0,2>(a));
    write_mat_element<0,3>(r,mat_traits<A>::template read_element<0,3>(a));
    write_mat_element<1,0>(r,mat_traits<A>::template read_element<1,0>(a));
    write_mat_element<1,1>(r,mat_traits<A>::template read_element<1,1>(a));
    write_mat_element<1,2>(r,mat_traits<A>::template read_element<1,2>(a));
    write_mat_element<1,3>(r,mat_traits<A>::template read_element<1,3>(a));
    write_mat_element<2,0>(r,mat_traits<A>::template read_element<2,0>(a));
    write_mat_element<2,1>(r,mat_traits<A>::template read_element<2,1>(a));
    write_mat_element<2,2>(r,mat_traits<A>::template read_element<2,2>(a));
    write_mat_element<2,3>(r,mat_traits<A>::template read_element<2,3>(a));
    write_mat_element<3,0>(r,mat_traits<A>::template read_element<3,0>(a));
    write_mat_element<3,1>(r,mat_traits<A>::template read_element<3,1>(a));
    write_mat_element<3,2>(r,mat_traits<A>::template read_element<3,2>(a));
    write_mat_element<3,3>(r,mat_traits<A>::template read_element<3,3>(a));
    return r;
    }

template <class R,class A>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE
typename enable_if_c<
    is_mat<R>::value && is_quat<A>::value &&
    mat_traits<R>::rows==4 && mat_traits<R>::cols==4,
    R>::type
convert_to( A const & q )
    {
    typedef typename mat_traits<R>::scalar_type T;
    T const a=quat_traits<A>::template read_element<0>(q);
    T const b=quat_traits<A>::template read_element<1>(q);
    T const c=quat_traits<A>::template read_element<2>(q);
    T const d=quat_traits<A>::template read_element<3>(q);
    T const bb = b*b;
    T const cc = c*c;
    T const dd = d*d;
    T const bc = b*c;
    T const bd = b*d;
    T const cd = c*d;
    T const ab = a*b;
    T const ac = a*c;
    T const ad = a*d;
    T const zero = scalar_traits<T>::value(0);
    T const one = scalar_traits<T>::value(1);
    T const two = one+one;
    R r;
    write_mat_element<0,0>(r,one - two*(cc+dd));
    write_mat_element<0,1>(r,two*(bc-ad));
    write_mat_element<0,2>(r,two*(bd+ac));
    write_mat_element<0,3>(r,zero);
    write_mat_element<1,0>(r,two*(bc+ad));
    write_mat_element<1,1>(r,one - two*(bb+dd));
    write_mat_element<1,2>(r,two*(cd-ab));
    write_mat_element<1,3>(r,zero);
    write_mat_element<2,0>(r,two*(bd-ac));
    write_mat_element<2,1>(r,two*(cd+ab));
    write_mat_element<2,2>(r,one - two*(bb+cc));
    write_mat_element<2,3>(r,zero);
    write_mat_element<3,0>(r,zero);
    write_mat_element<3,1>(r,zero);
    write_mat_element<3,2>(r,zero);
    write_mat_element<3,3>(r,one);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::convert_to;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct convert_to_m_defined;

    template <>
    struct
    convert_to_m_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class R,class A>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<R>::rows==4 && mat_traits<A>::rows==4 &&
    mat_traits<R>::cols==1 && mat_traits<A>::cols==1,
    R>::type
convert_to( A const & a )
    {
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a));
    write_mat_element<1,0>(r,mat_traits<A>::template read_element<1,0>(a));
    write_mat_element<2,0>(r,mat_traits<A>::template read_element<2,0>(a));
    write_mat_element<3,0>(r,mat_traits<A>::template read_element<3,0>(a));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::convert_to;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct convert_to_m_defined;

    template <>
    struct
    convert_to_m_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class R,class A>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<R>::rows==1 && mat_traits<A>::rows==1 &&
    mat_traits<R>::cols==4 && mat_traits<A>::cols==4,
    R>::type
convert_to( A const & a )
    {
    R r;
    write_mat_element<0,0>(r,mat_traits<A>::template read_element<0,0>(a));
    write_mat_element<0,1>(r,mat_traits<A>::template read_element<0,1>(a));
    write_mat_element<0,2>(r,mat_traits<A>::template read_element<0,2>(a));
    write_mat_element<0,3>(r,mat_traits<A>::template read_element<0,3>(a));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::convert_to;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct convert_to_m_defined;

    template <>
    struct
    convert_to_m_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    bool>::type
operator==( A const & a, B const & b )
    {
    return
        mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b) &&
        mat_traits<A>::template read_element<0,1>(a)==mat_traits<B>::template read_element<0,1>(b) &&
        mat_traits<A>::template read_element<0,2>(a)==mat_traits<B>::template read_element<0,2>(b) &&
        mat_traits<A>::template read_element<0,3>(a)==mat_traits<B>::template read_element<0,3>(b) &&
        mat_traits<A>::template read_element<1,0>(a)==mat_traits<B>::template read_element<1,0>(b) &&
        mat_traits<A>::template read_element<1,1>(a)==mat_traits<B>::template read_element<1,1>(b) &&
        mat_traits<A>::template read_element<1,2>(a)==mat_traits<B>::template read_element<1,2>(b) &&
        mat_traits<A>::template read_element<1,3>(a)==mat_traits<B>::template read_element<1,3>(b) &&
        mat_traits<A>::template read_element<2,0>(a)==mat_traits<B>::template read_element<2,0>(b) &&
        mat_traits<A>::template read_element<2,1>(a)==mat_traits<B>::template read_element<2,1>(b) &&
        mat_traits<A>::template read_element<2,2>(a)==mat_traits<B>::template read_element<2,2>(b) &&
        mat_traits<A>::template read_element<2,3>(a)==mat_traits<B>::template read_element<2,3>(b) &&
        mat_traits<A>::template read_element<3,0>(a)==mat_traits<B>::template read_element<3,0>(b) &&
        mat_traits<A>::template read_element<3,1>(a)==mat_traits<B>::template read_element<3,1>(b) &&
        mat_traits<A>::template read_element<3,2>(a)==mat_traits<B>::template read_element<3,2>(b) &&
        mat_traits<A>::template read_element<3,3>(a)==mat_traits<B>::template read_element<3,3>(b);
    }

namespace
sfinae
    {
    using ::boost::qvm::operator==;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct eq_mm_defined;

    template <>
    struct
    eq_mm_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
    bool>::type
operator==( A const & a, B const & b )
    {
    return
        mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b) &&
        mat_traits<A>::template read_element<1,0>(a)==mat_traits<B>::template read_element<1,0>(b) &&
        mat_traits<A>::template read_element<2,0>(a)==mat_traits<B>::template read_element<2,0>(b) &&
        mat_traits<A>::template read_element<3,0>(a)==mat_traits<B>::template read_element<3,0>(b);
    }

namespace
sfinae
    {
    using ::boost::qvm::operator==;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct eq_mm_defined;

    template <>
    struct
    eq_mm_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    bool>::type
operator==( A const & a, B const & b )
    {
    return
        mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b) &&
        mat_traits<A>::template read_element<0,1>(a)==mat_traits<B>::template read_element<0,1>(b) &&
        mat_traits<A>::template read_element<0,2>(a)==mat_traits<B>::template read_element<0,2>(b) &&
        mat_traits<A>::template read_element<0,3>(a)==mat_traits<B>::template read_element<0,3>(b);
    }

namespace
sfinae
    {
    using ::boost::qvm::operator==;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct eq_mm_defined;

    template <>
    struct
    eq_mm_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    bool>::type
operator!=( A const & a, B const & b )
    {
    return
        !(mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b)) ||
        !(mat_traits<A>::template read_element<0,1>(a)==mat_traits<B>::template read_element<0,1>(b)) ||
        !(mat_traits<A>::template read_element<0,2>(a)==mat_traits<B>::template read_element<0,2>(b)) ||
        !(mat_traits<A>::template read_element<0,3>(a)==mat_traits<B>::template read_element<0,3>(b)) ||
        !(mat_traits<A>::template read_element<1,0>(a)==mat_traits<B>::template read_element<1,0>(b)) ||
        !(mat_traits<A>::template read_element<1,1>(a)==mat_traits<B>::template read_element<1,1>(b)) ||
        !(mat_traits<A>::template read_element<1,2>(a)==mat_traits<B>::template read_element<1,2>(b)) ||
        !(mat_traits<A>::template read_element<1,3>(a)==mat_traits<B>::template read_element<1,3>(b)) ||
        !(mat_traits<A>::template read_element<2,0>(a)==mat_traits<B>::template read_element<2,0>(b)) ||
        !(mat_traits<A>::template read_element<2,1>(a)==mat_traits<B>::template read_element<2,1>(b)) ||
        !(mat_traits<A>::template read_element<2,2>(a)==mat_traits<B>::template read_element<2,2>(b)) ||
        !(mat_traits<A>::template read_element<2,3>(a)==mat_traits<B>::template read_element<2,3>(b)) ||
        !(mat_traits<A>::template read_element<3,0>(a)==mat_traits<B>::template read_element<3,0>(b)) ||
        !(mat_traits<A>::template read_element<3,1>(a)==mat_traits<B>::template read_element<3,1>(b)) ||
        !(mat_traits<A>::template read_element<3,2>(a)==mat_traits<B>::template read_element<3,2>(b)) ||
        !(mat_traits<A>::template read_element<3,3>(a)==mat_traits<B>::template read_element<3,3>(b));
    }

namespace
sfinae
    {
    using ::boost::qvm::operator!=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct neq_mm_defined;

    template <>
    struct
    neq_mm_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
    bool>::type
operator!=( A const & a, B const & b )
    {
    return
        !(mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b)) ||
        !(mat_traits<A>::template read_element<1,0>(a)==mat_traits<B>::template read_element<1,0>(b)) ||
        !(mat_traits<A>::template read_element<2,0>(a)==mat_traits<B>::template read_element<2,0>(b)) ||
        !(mat_traits<A>::template read_element<3,0>(a)==mat_traits<B>::template read_element<3,0>(b));
    }

namespace
sfinae
    {
    using ::boost::qvm::operator!=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct neq_mm_defined;

    template <>
    struct
    neq_mm_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    bool>::type
operator!=( A const & a, B const & b )
    {
    return
        !(mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b)) ||
        !(mat_traits<A>::template read_element<0,1>(a)==mat_traits<B>::template read_element<0,1>(b)) ||
        !(mat_traits<A>::template read_element<0,2>(a)==mat_traits<B>::template read_element<0,2>(b)) ||
        !(mat_traits<A>::template read_element<0,3>(a)==mat_traits<B>::template read_element<0,3>(b));
    }

namespace
sfinae
    {
    using ::boost::qvm::operator!=;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct neq_mm_defined;

    template <>
    struct
    neq_mm_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==4,
    deduce_mat<A> >::type
operator-( A const & a )
    {
    typedef typename deduce_mat<A>::type R;
    R r;
    write_mat_element<0,0>(r,-mat_traits<A>::template read_element<0,0>(a));
    write_mat_element<0,1>(r,-mat_traits<A>::template read_element<0,1>(a));
    write_mat_element<0,2>(r,-mat_traits<A>::template read_element<0,2>(a));
    write_mat_element<0,3>(r,-mat_traits<A>::template read_element<0,3>(a));
    write_mat_element<1,0>(r,-mat_traits<A>::template read_element<1,0>(a));
    write_mat_element<1,1>(r,-mat_traits<A>::template read_element<1,1>(a));
    write_mat_element<1,2>(r,-mat_traits<A>::template read_element<1,2>(a));
    write_mat_element<1,3>(r,-mat_traits<A>::template read_element<1,3>(a));
    write_mat_element<2,0>(r,-mat_traits<A>::template read_element<2,0>(a));
    write_mat_element<2,1>(r,-mat_traits<A>::template read_element<2,1>(a));
    write_mat_element<2,2>(r,-mat_traits<A>::template read_element<2,2>(a));
    write_mat_element<2,3>(r,-mat_traits<A>::template read_element<2,3>(a));
    write_mat_element<3,0>(r,-mat_traits<A>::template read_element<3,0>(a));
    write_mat_element<3,1>(r,-mat_traits<A>::template read_element<3,1>(a));
    write_mat_element<3,2>(r,-mat_traits<A>::template read_element<3,2>(a));
    write_mat_element<3,3>(r,-mat_traits<A>::template read_element<3,3>(a));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct minus_m_defined;

    template <>
    struct
    minus_m_defined<4,4>
        {
        static bool const value=true;
        };
    }

template <class A>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==1,
    deduce_mat<A> >::type
operator-( A const & a )
    {
    typedef typename deduce_mat<A>::type R;
    R r;
    write_mat_element<0,0>(r,-mat_traits<A>::template read_element<0,0>(a));
    write_mat_element<1,0>(r,-mat_traits<A>::template read_element<1,0>(a));
    write_mat_element<2,0>(r,-mat_traits<A>::template read_element<2,0>(a));
    write_mat_element<3,0>(r,-mat_traits<A>::template read_element<3,0>(a));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct minus_m_defined;

    template <>
    struct
    minus_m_defined<4,1>
        {
        static bool const value=true;
        };
    }

template <class A>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<A>::cols==4,
    deduce_mat<A> >::type
operator-( A const & a )
    {
    typedef typename deduce_mat<A>::type R;
    R r;
    write_mat_element<0,0>(r,-mat_traits<A>::template read_element<0,0>(a));
    write_mat_element<0,1>(r,-mat_traits<A>::template read_element<0,1>(a));
    write_mat_element<0,2>(r,-mat_traits<A>::template read_element<0,2>(a));
    write_mat_element<0,3>(r,-mat_traits<A>::template read_element<0,3>(a));
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator-;
    }

namespace
qvm_detail
    {
    template <int R,int C>
    struct minus_m_defined;

    template <>
    struct
    minus_m_defined<1,4>
        {
        static bool const value=true;
        };
    }

template <class A>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==4,
    typename mat_traits<A>::scalar_type>::type
determinant( A const & a )
    {
    typedef typename mat_traits<A>::scalar_type T;
    T const a00=mat_traits<A>::template read_element<0,0>(a);
    T const a01=mat_traits<A>::template read_element<0,1>(a);
    T const a02=mat_traits<A>::template read_element<0,2>(a);
    T const a03=mat_traits<A>::template read_element<0,3>(a);
    T const a10=mat_traits<A>::template read_element<1,0>(a);
    T const a11=mat_traits<A>::template read_element<1,1>(a);
    T const a12=mat_traits<A>::template read_element<1,2>(a);
    T const a13=mat_traits<A>::template read_element<1,3>(a);
    T const a20=mat_traits<A>::template read_element<2,0>(a);
    T const a21=mat_traits<A>::template read_element<2,1>(a);
    T const a22=mat_traits<A>::template read_element<2,2>(a);
    T const a23=mat_traits<A>::template read_element<2,3>(a);
    T const a30=mat_traits<A>::template read_element<3,0>(a);
    T const a31=mat_traits<A>::template read_element<3,1>(a);
    T const a32=mat_traits<A>::template read_element<3,2>(a);
    T const a33=mat_traits<A>::template read_element<3,3>(a);
    T det=(a00*(a11*(a22*a33-a23*a32)-a12*(a21*a33-a23*a31)+a13*(a21*a32-a22*a31))-a01*(a10*(a22*a33-a23*a32)-a12*(a20*a33-a23*a30)+a13*(a20*a32-a22*a30))+a02*(a10*(a21*a33-a23*a31)-a11*(a20*a33-a23*a30)+a13*(a20*a31-a21*a30))-a03*(a10*(a21*a32-a22*a31)-a11*(a20*a32-a22*a30)+a12*(a20*a31-a21*a30)));
    return det;
    }

namespace
sfinae
    {
    using ::boost::qvm::determinant;
    }

namespace
qvm_detail
    {
    template <int D>
    struct determinant_defined;

    template <>
    struct
    determinant_defined<4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==4 && is_scalar<B>::value,
    deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols> >::type
inverse( A const & a, B det )
    {
    typedef typename mat_traits<A>::scalar_type T;
    BOOST_QVM_ASSERT(det!=scalar_traits<B>::value(0));
    T const a00=mat_traits<A>::template read_element<0,0>(a);
    T const a01=mat_traits<A>::template read_element<0,1>(a);
    T const a02=mat_traits<A>::template read_element<0,2>(a);
    T const a03=mat_traits<A>::template read_element<0,3>(a);
    T const a10=mat_traits<A>::template read_element<1,0>(a);
    T const a11=mat_traits<A>::template read_element<1,1>(a);
    T const a12=mat_traits<A>::template read_element<1,2>(a);
    T const a13=mat_traits<A>::template read_element<1,3>(a);
    T const a20=mat_traits<A>::template read_element<2,0>(a);
    T const a21=mat_traits<A>::template read_element<2,1>(a);
    T const a22=mat_traits<A>::template read_element<2,2>(a);
    T const a23=mat_traits<A>::template read_element<2,3>(a);
    T const a30=mat_traits<A>::template read_element<3,0>(a);
    T const a31=mat_traits<A>::template read_element<3,1>(a);
    T const a32=mat_traits<A>::template read_element<3,2>(a);
    T const a33=mat_traits<A>::template read_element<3,3>(a);
    T const f=scalar_traits<T>::value(1)/det;
    typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols>::type R;
    R r;
    write_mat_element<0,0>(r, f*(a11*(a22*a33-a23*a32)-a12*(a21*a33-a23*a31)+a13*(a21*a32-a22*a31)));
    write_mat_element<0,1>(r,-f*(a01*(a22*a33-a23*a32)-a02*(a21*a33-a23*a31)+a03*(a21*a32-a22*a31)));
    write_mat_element<0,2>(r, f*(a01*(a12*a33-a13*a32)-a02*(a11*a33-a13*a31)+a03*(a11*a32-a12*a31)));
    write_mat_element<0,3>(r,-f*(a01*(a12*a23-a13*a22)-a02*(a11*a23-a13*a21)+a03*(a11*a22-a12*a21)));
    write_mat_element<1,0>(r,-f*(a10*(a22*a33-a23*a32)-a12*(a20*a33-a23*a30)+a13*(a20*a32-a22*a30)));
    write_mat_element<1,1>(r, f*(a00*(a22*a33-a23*a32)-a02*(a20*a33-a23*a30)+a03*(a20*a32-a22*a30)));
    write_mat_element<1,2>(r,-f*(a00*(a12*a33-a13*a32)-a02*(a10*a33-a13*a30)+a03*(a10*a32-a12*a30)));
    write_mat_element<1,3>(r, f*(a00*(a12*a23-a13*a22)-a02*(a10*a23-a13*a20)+a03*(a10*a22-a12*a20)));
    write_mat_element<2,0>(r, f*(a10*(a21*a33-a23*a31)-a11*(a20*a33-a23*a30)+a13*(a20*a31-a21*a30)));
    write_mat_element<2,1>(r,-f*(a00*(a21*a33-a23*a31)-a01*(a20*a33-a23*a30)+a03*(a20*a31-a21*a30)));
    write_mat_element<2,2>(r, f*(a00*(a11*a33-a13*a31)-a01*(a10*a33-a13*a30)+a03*(a10*a31-a11*a30)));
    write_mat_element<2,3>(r,-f*(a00*(a11*a23-a13*a21)-a01*(a10*a23-a13*a20)+a03*(a10*a21-a11*a20)));
    write_mat_element<3,0>(r,-f*(a10*(a21*a32-a22*a31)-a11*(a20*a32-a22*a30)+a12*(a20*a31-a21*a30)));
    write_mat_element<3,1>(r, f*(a00*(a21*a32-a22*a31)-a01*(a20*a32-a22*a30)+a02*(a20*a31-a21*a30)));
    write_mat_element<3,2>(r,-f*(a00*(a11*a32-a12*a31)-a01*(a10*a32-a12*a30)+a02*(a10*a31-a11*a30)));
    write_mat_element<3,3>(r, f*(a00*(a11*a22-a12*a21)-a01*(a10*a22-a12*a20)+a02*(a10*a21-a11*a20)));
    return r;
    }

template <class A>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<A>::cols==4,
    deduce_mat<A> >::type
inverse( A const & a )
    {
    typedef typename mat_traits<A>::scalar_type T;
    T det=determinant(a);
    if( det==scalar_traits<T>::value(0) )
        BOOST_QVM_THROW_EXCEPTION(zero_determinant_error());
    return inverse(a,det);
    }

namespace
sfinae
    {
    using ::boost::qvm::inverse;
    }

namespace
qvm_detail
    {
    template <int D>
    struct inverse_m_defined;

    template <>
    struct
    inverse_m_defined<4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    deduce_mat2<A,B,4,4> >::type
operator*( A const & a, B const & b )
    {
    typedef typename mat_traits<A>::scalar_type Ta;
    typedef typename mat_traits<B>::scalar_type Tb;
    Ta const a00 = mat_traits<A>::template read_element<0,0>(a);
    Ta const a01 = mat_traits<A>::template read_element<0,1>(a);
    Ta const a02 = mat_traits<A>::template read_element<0,2>(a);
    Ta const a03 = mat_traits<A>::template read_element<0,3>(a);
    Ta const a10 = mat_traits<A>::template read_element<1,0>(a);
    Ta const a11 = mat_traits<A>::template read_element<1,1>(a);
    Ta const a12 = mat_traits<A>::template read_element<1,2>(a);
    Ta const a13 = mat_traits<A>::template read_element<1,3>(a);
    Ta const a20 = mat_traits<A>::template read_element<2,0>(a);
    Ta const a21 = mat_traits<A>::template read_element<2,1>(a);
    Ta const a22 = mat_traits<A>::template read_element<2,2>(a);
    Ta const a23 = mat_traits<A>::template read_element<2,3>(a);
    Ta const a30 = mat_traits<A>::template read_element<3,0>(a);
    Ta const a31 = mat_traits<A>::template read_element<3,1>(a);
    Ta const a32 = mat_traits<A>::template read_element<3,2>(a);
    Ta const a33 = mat_traits<A>::template read_element<3,3>(a);
    Tb const b00 = mat_traits<B>::template read_element<0,0>(b);
    Tb const b01 = mat_traits<B>::template read_element<0,1>(b);
    Tb const b02 = mat_traits<B>::template read_element<0,2>(b);
    Tb const b03 = mat_traits<B>::template read_element<0,3>(b);
    Tb const b10 = mat_traits<B>::template read_element<1,0>(b);
    Tb const b11 = mat_traits<B>::template read_element<1,1>(b);
    Tb const b12 = mat_traits<B>::template read_element<1,2>(b);
    Tb const b13 = mat_traits<B>::template read_element<1,3>(b);
    Tb const b20 = mat_traits<B>::template read_element<2,0>(b);
    Tb const b21 = mat_traits<B>::template read_element<2,1>(b);
    Tb const b22 = mat_traits<B>::template read_element<2,2>(b);
    Tb const b23 = mat_traits<B>::template read_element<2,3>(b);
    Tb const b30 = mat_traits<B>::template read_element<3,0>(b);
    Tb const b31 = mat_traits<B>::template read_element<3,1>(b);
    Tb const b32 = mat_traits<B>::template read_element<3,2>(b);
    Tb const b33 = mat_traits<B>::template read_element<3,3>(b);
    typedef typename deduce_mat2<A,B,4,4>::type R;
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==4);
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==4);
    R r;
    write_mat_element<0,0>(r,a00*b00+a01*b10+a02*b20+a03*b30);
    write_mat_element<0,1>(r,a00*b01+a01*b11+a02*b21+a03*b31);
    write_mat_element<0,2>(r,a00*b02+a01*b12+a02*b22+a03*b32);
    write_mat_element<0,3>(r,a00*b03+a01*b13+a02*b23+a03*b33);
    write_mat_element<1,0>(r,a10*b00+a11*b10+a12*b20+a13*b30);
    write_mat_element<1,1>(r,a10*b01+a11*b11+a12*b21+a13*b31);
    write_mat_element<1,2>(r,a10*b02+a11*b12+a12*b22+a13*b32);
    write_mat_element<1,3>(r,a10*b03+a11*b13+a12*b23+a13*b33);
    write_mat_element<2,0>(r,a20*b00+a21*b10+a22*b20+a23*b30);
    write_mat_element<2,1>(r,a20*b01+a21*b11+a22*b21+a23*b31);
    write_mat_element<2,2>(r,a20*b02+a21*b12+a22*b22+a23*b32);
    write_mat_element<2,3>(r,a20*b03+a21*b13+a22*b23+a23*b33);
    write_mat_element<3,0>(r,a30*b00+a31*b10+a32*b20+a33*b30);
    write_mat_element<3,1>(r,a30*b01+a31*b11+a32*b21+a33*b31);
    write_mat_element<3,2>(r,a30*b02+a31*b12+a32*b22+a33*b32);
    write_mat_element<3,3>(r,a30*b03+a31*b13+a32*b23+a33*b33);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int R,int /*CR*/,int C>
    struct mul_mm_defined;

    template <>
    struct
    mul_mm_defined<4,4,4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    A &>::type
operator*=( A & a, B const & b )
    {
    typedef typename mat_traits<A>::scalar_type Ta;
    typedef typename mat_traits<B>::scalar_type Tb;
    Ta const a00 = mat_traits<A>::template read_element<0,0>(a);
    Ta const a01 = mat_traits<A>::template read_element<0,1>(a);
    Ta const a02 = mat_traits<A>::template read_element<0,2>(a);
    Ta const a03 = mat_traits<A>::template read_element<0,3>(a);
    Ta const a10 = mat_traits<A>::template read_element<1,0>(a);
    Ta const a11 = mat_traits<A>::template read_element<1,1>(a);
    Ta const a12 = mat_traits<A>::template read_element<1,2>(a);
    Ta const a13 = mat_traits<A>::template read_element<1,3>(a);
    Ta const a20 = mat_traits<A>::template read_element<2,0>(a);
    Ta const a21 = mat_traits<A>::template read_element<2,1>(a);
    Ta const a22 = mat_traits<A>::template read_element<2,2>(a);
    Ta const a23 = mat_traits<A>::template read_element<2,3>(a);
    Ta const a30 = mat_traits<A>::template read_element<3,0>(a);
    Ta const a31 = mat_traits<A>::template read_element<3,1>(a);
    Ta const a32 = mat_traits<A>::template read_element<3,2>(a);
    Ta const a33 = mat_traits<A>::template read_element<3,3>(a);
    Tb const b00 = mat_traits<B>::template read_element<0,0>(b);
    Tb const b01 = mat_traits<B>::template read_element<0,1>(b);
    Tb const b02 = mat_traits<B>::template read_element<0,2>(b);
    Tb const b03 = mat_traits<B>::template read_element<0,3>(b);
    Tb const b10 = mat_traits<B>::template read_element<1,0>(b);
    Tb const b11 = mat_traits<B>::template read_element<1,1>(b);
    Tb const b12 = mat_traits<B>::template read_element<1,2>(b);
    Tb const b13 = mat_traits<B>::template read_element<1,3>(b);
    Tb const b20 = mat_traits<B>::template read_element<2,0>(b);
    Tb const b21 = mat_traits<B>::template read_element<2,1>(b);
    Tb const b22 = mat_traits<B>::template read_element<2,2>(b);
    Tb const b23 = mat_traits<B>::template read_element<2,3>(b);
    Tb const b30 = mat_traits<B>::template read_element<3,0>(b);
    Tb const b31 = mat_traits<B>::template read_element<3,1>(b);
    Tb const b32 = mat_traits<B>::template read_element<3,2>(b);
    Tb const b33 = mat_traits<B>::template read_element<3,3>(b);
    write_mat_element<0,0>(a,a00*b00+a01*b10+a02*b20+a03*b30);
    write_mat_element<0,1>(a,a00*b01+a01*b11+a02*b21+a03*b31);
    write_mat_element<0,2>(a,a00*b02+a01*b12+a02*b22+a03*b32);
    write_mat_element<0,3>(a,a00*b03+a01*b13+a02*b23+a03*b33);
    write_mat_element<1,0>(a,a10*b00+a11*b10+a12*b20+a13*b30);
    write_mat_element<1,1>(a,a10*b01+a11*b11+a12*b21+a13*b31);
    write_mat_element<1,2>(a,a10*b02+a11*b12+a12*b22+a13*b32);
    write_mat_element<1,3>(a,a10*b03+a11*b13+a12*b23+a13*b33);
    write_mat_element<2,0>(a,a20*b00+a21*b10+a22*b20+a23*b30);
    write_mat_element<2,1>(a,a20*b01+a21*b11+a22*b21+a23*b31);
    write_mat_element<2,2>(a,a20*b02+a21*b12+a22*b22+a23*b32);
    write_mat_element<2,3>(a,a20*b03+a21*b13+a22*b23+a23*b33);
    write_mat_element<3,0>(a,a30*b00+a31*b10+a32*b20+a33*b30);
    write_mat_element<3,1>(a,a30*b01+a31*b11+a32*b21+a33*b31);
    write_mat_element<3,2>(a,a30*b02+a31*b12+a32*b22+a33*b32);
    write_mat_element<3,3>(a,a30*b03+a31*b13+a32*b23+a33*b33);
    return a;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*=;
    }

namespace
qvm_detail
    {
    template <int D>
    struct mul_eq_mm_defined;

    template <>
    struct
    mul_eq_mm_defined<4>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==4 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==1,
    deduce_mat2<A,B,4,1> >::type
operator*( A const & a, B const & b )
    {
    typedef typename mat_traits<A>::scalar_type Ta;
    typedef typename mat_traits<B>::scalar_type Tb;
    Ta const a00 = mat_traits<A>::template read_element<0,0>(a);
    Ta const a01 = mat_traits<A>::template read_element<0,1>(a);
    Ta const a02 = mat_traits<A>::template read_element<0,2>(a);
    Ta const a03 = mat_traits<A>::template read_element<0,3>(a);
    Ta const a10 = mat_traits<A>::template read_element<1,0>(a);
    Ta const a11 = mat_traits<A>::template read_element<1,1>(a);
    Ta const a12 = mat_traits<A>::template read_element<1,2>(a);
    Ta const a13 = mat_traits<A>::template read_element<1,3>(a);
    Ta const a20 = mat_traits<A>::template read_element<2,0>(a);
    Ta const a21 = mat_traits<A>::template read_element<2,1>(a);
    Ta const a22 = mat_traits<A>::template read_element<2,2>(a);
    Ta const a23 = mat_traits<A>::template read_element<2,3>(a);
    Ta const a30 = mat_traits<A>::template read_element<3,0>(a);
    Ta const a31 = mat_traits<A>::template read_element<3,1>(a);
    Ta const a32 = mat_traits<A>::template read_element<3,2>(a);
    Ta const a33 = mat_traits<A>::template read_element<3,3>(a);
    Tb const b00 = mat_traits<B>::template read_element<0,0>(b);
    Tb const b10 = mat_traits<B>::template read_element<1,0>(b);
    Tb const b20 = mat_traits<B>::template read_element<2,0>(b);
    Tb const b30 = mat_traits<B>::template read_element<3,0>(b);
    typedef typename deduce_mat2<A,B,4,1>::type R;
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==4);
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==1);
    R r;
    write_mat_element<0,0>(r,a00*b00+a01*b10+a02*b20+a03*b30);
    write_mat_element<1,0>(r,a10*b00+a11*b10+a12*b20+a13*b30);
    write_mat_element<2,0>(r,a20*b00+a21*b10+a22*b20+a23*b30);
    write_mat_element<3,0>(r,a30*b00+a31*b10+a32*b20+a33*b30);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int R,int /*CR*/,int C>
    struct mul_mm_defined;

    template <>
    struct
    mul_mm_defined<4,4,1>
        {
        static bool const value=true;
        };
    }

template <class A,class B>
BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS
typename lazy_enable_if_c<
    mat_traits<A>::rows==1 && mat_traits<B>::rows==4 &&
    mat_traits<A>::cols==4 && mat_traits<B>::cols==4,
    deduce_mat2<A,B,1,4> >::type
operator*( A const & a, B const & b )
    {
    typedef typename mat_traits<A>::scalar_type Ta;
    typedef typename mat_traits<B>::scalar_type Tb;
    Ta const a00 = mat_traits<A>::template read_element<0,0>(a);
    Ta const a01 = mat_traits<A>::template read_element<0,1>(a);
    Ta const a02 = mat_traits<A>::template read_element<0,2>(a);
    Ta const a03 = mat_traits<A>::template read_element<0,3>(a);
    Tb const b00 = mat_traits<B>::template read_element<0,0>(b);
    Tb const b01 = mat_traits<B>::template read_element<0,1>(b);
    Tb const b02 = mat_traits<B>::template read_element<0,2>(b);
    Tb const b03 = mat_traits<B>::template read_element<0,3>(b);
    Tb const b10 = mat_traits<B>::template read_element<1,0>(b);
    Tb const b11 = mat_traits<B>::template read_element<1,1>(b);
    Tb const b12 = mat_traits<B>::template read_element<1,2>(b);
    Tb const b13 = mat_traits<B>::template read_element<1,3>(b);
    Tb const b20 = mat_traits<B>::template read_element<2,0>(b);
    Tb const b21 = mat_traits<B>::template read_element<2,1>(b);
    Tb const b22 = mat_traits<B>::template read_element<2,2>(b);
    Tb const b23 = mat_traits<B>::template read_element<2,3>(b);
    Tb const b30 = mat_traits<B>::template read_element<3,0>(b);
    Tb const b31 = mat_traits<B>::template read_element<3,1>(b);
    Tb const b32 = mat_traits<B>::template read_element<3,2>(b);
    Tb const b33 = mat_traits<B>::template read_element<3,3>(b);
    typedef typename deduce_mat2<A,B,1,4>::type R;
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==1);
    BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==4);
    R r;
    write_mat_element<0,0>(r,a00*b00+a01*b10+a02*b20+a03*b30);
    write_mat_element<0,1>(r,a00*b01+a01*b11+a02*b21+a03*b31);
    write_mat_element<0,2>(r,a00*b02+a01*b12+a02*b22+a03*b32);
    write_mat_element<0,3>(r,a00*b03+a01*b13+a02*b23+a03*b33);
    return r;
    }

namespace
sfinae
    {
    using ::boost::qvm::operator*;
    }

namespace
qvm_detail
    {
    template <int R,int /*CR*/,int C>
    struct mul_mm_defined;

    template <>
    struct
    mul_mm_defined<1,4,4>
        {
        static bool const value=true;
        };
    }

} }

#endif


#include <exception>

#include <Eigen/Dense>
#include <drake_robot_control/task_space_trajectory_plan.h>
#include <drake_robot_control/utils.h>

// ROS
#include <ros/console.h>

namespace drake {
namespace robot_plan_runner {

typedef spartan::drake_robot_control::ForceGuard ForceGuard;

/// Notation
/// H_JI: is the homogeneous transform from I to J
/// T_JI_K: twist of I with respect to J expressed in frame K

/// This plan implements a task space controller
/// The treatment here follows the notation of [1] and [2]
/// [1] Design of a Momentum based Control Framework and its application
/// to the Humanoid Robot Atlas
/// [2] Proportional Derivative (PD) Control on the Euclidean Group
/// E_r is the reference trajectory for the body frame
/// E is the current frame of the body frame
/// Since the kuka is velocity controlled this is a first order system
/// The error term is given be e = (H_WEr)^{-1} H_WE
/// H_WE represents the homogenous transform from E to W
/// The dynamics of the error term are
/// V^e = V - Ad_{H_WE_inv}(V_d) where V_d is the velocity of the reference
/// trajectory. A stabilizing controller for V^e just drives it to the
/// origin/identity
/// using a PD controller
/// (rotation)  K_{p,w} log_{SO(3)}(R_EEr)
/// (translation) K_{p,v} p_{E_r}^E
/// and also feed forward
/// (feed-forward) T_EW_Er
/// T_EW_E = PD + feed-forward

typedef drake::TwistVector<double> TwistVectord;

void EndEffectorOriginTrajectoryPlan::Step(
    const Eigen::Ref<const Eigen::VectorXd> &x,
    const Eigen::Ref<const Eigen::VectorXd> &tau_external, double t,
    Eigen::VectorXd *const q_commanded, Eigen::VectorXd *const v_commanded,
    Eigen::VectorXd *const tau_commanded) {

  *tau_commanded = Eigen::VectorXd::Zero(this->get_num_positions());
  DRAKE_ASSERT(t >= 0);

  PlanStatus not_started_status = PlanStatus::NOT_STARTED;
  PlanStatus running_status = PlanStatus::RUNNING;

  if (plan_status_.compare_exchange_strong(not_started_status,
                                           PlanStatus::RUNNING)) {
    std::cout << "plan status was NOT_STARTED, setting it to RUNNING"
              << std::endl;
  }

  if (this->is_stopped()) {
    *q_commanded = q_commanded_prev_;
    *tau_commanded = Eigen::VectorXd::Zero(this->get_num_positions());
    return;
  }

  // if we reach here then plan should be in state RUNNING

  // Eigen::VectorXd q = x.head(this->get_num_positions());
  // Instead of the actual q, use the last commanded q
  Eigen::VectorXd q_measured = x.head(this->get_num_positions());
  Eigen::VectorXd q = q_commanded_prev_;
  Eigen::VectorXd v = x.tail(this->get_num_velocities());

  Eigen::Vector3d xyz_ee_ref = traj_.value(t);     // expressed in world
  Eigen::Vector3d xyz_d_ee_ref = traj_d_.value(t); // expressed in world

  double t_fraction = std::min(t / this->duration(), 1.0);
  math::RotationMatrixd R_WEr(
      quat_WE_initial_.slerp(t_fraction, quat_WE_final_));
  math::RotationMatrixd R_ErW = R_WEr.inverse();

  // compute KinematicsCache off of measured states
  // for use in computing the force guards
  cache_measured_state_.initialize(q_measured, v);
  tree_->doKinematics(cache_measured_state_);

  // Check the external force guards
  if (guard_container_) {
    std::pair<bool, std::pair<double, std::shared_ptr<ForceGuard>>> result =
        guard_container_->EvaluateGuards(cache_measured_state_, q_measured,
                                         tau_external);

    bool guard_triggered = result.first;

    if (guard_triggered) {
      std::cout << "Force Guard Triggered, stopping plan" << std::endl;
      std::cout << "ForceGuardType: " << result.second.second->get_type()
                << std::endl;
      plan_status_ = PlanStatus::STOPPED_BY_FORCE_GUARD;
      this->SetPlanFinished();

      *q_commanded = q_commanded_prev_;
      *tau_commanded = Eigen::VectorXd::Zero(this->get_num_positions());
      return;
    }
  }

  cache_.initialize(q, v);
  tree_->doKinematics(cache_);

  J_ee_E_ = tree_->geometricJacobian(cache_, idx_world_, idx_ee_, idx_ee_);
  J_ee_W_ = tree_->geometricJacobian(cache_, idx_world_, idx_ee_, idx_world_);

  PlanStatus plan_status = this->get_plan_status();
  if (this->get_plan_status() == PlanStatus::RUNNING) {

    if (t > this->duration()) {
      // if we arrive here, it means that plan status was
      // RUNNING and t > this->duration()
      if (plan_status_.compare_exchange_strong(running_status,
                                               PlanStatus::FINISHED_NORMALLY)) {
        ROS_INFO("plan finished normally");
        // notify the condition variable
        q_command_final_ = q_commanded_prev_;
        this->SetPlanFinished();
      }
    }
  }

  H_WEr_.set_rotation(R_WEr);
  H_WEr_.set_translation(xyz_ee_ref);
  Eigen::Isometry3d H_WEr = H_WEr_.GetAsIsometry3();

  H_WE_ = tree_->CalcBodyPoseInWorldFrame(cache_, tree_->get_body(idx_ee_));

  Eigen::Isometry3d H_EW = H_WE_.inverse();
  Eigen::Isometry3d H_EEr = H_EW * H_WEr;

  // Compute the PD part of the control
  // K_{p,w} log_{SO(3)}(R_EEr)
  Eigen::Vector3d log_R_EEr =
      spartan::drake_robot_control::utils::LogSO3(H_EEr.linear());

  TwistVectord twist_pd;
  // we need to do elementwise multiplication, hence the use of array instead
  // of Vector
  twist_pd.head(3) = this->kp_rotation_.array() * log_R_EEr.array();
  twist_pd.tail(3) =
      this->kp_translation_.array() * H_EEr.translation().array();

  // Compute the feed forward part of the control
  // Need twist of reference trajectory with respect to world
  // easiest to compute this as expressed in Er frame,
  // then transform that twist to E frame using adjoint
  TwistVectord T_WEr_Er;

  if (plan_status_ == PlanStatus::RUNNING) {
    // T_WEr_Er.head(3) = R_WEr * ang_velocity_WEr_W_;
    T_WEr_Er.head(3) = Eigen::Vector3d::Zero(); // hack for now
    T_WEr_Er.tail(3) = R_WEr * xyz_d_ee_ref;
  } else {
    // if the plan is finished, the feed forward should be zero
    T_WEr_Er.head(3) = Eigen::Vector3d::Zero();
    T_WEr_Er.tail(3) = Eigen::Vector3d::Zero();
  }

  Eigen::Matrix<double, 6, 6> Ad_H_EEr =
      spartan::drake_robot_control::utils::AdjointSE3(H_EEr.linear(),
                                                      H_EEr.translation());
  TwistVectord T_WEr_E = Ad_H_EEr * T_WEr_Er;

  // Total desired twist
  // Can add these this twists since both expressed in frame E
  TwistVectord T_WE_E_cmd = twist_pd + T_WEr_E;

  // q_dot_cmd = J_ee.pseudo_inverse()*T_WE_E_cmd
  auto svd = J_ee_E_.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
  // When computing the pseudoinverse, this ignores all singular
  // values smaller than this threshold. This is raised
  // from the Eigen default to create less jerky movements near
  // singularities.
  svd.setThreshold(0.01);
  Eigen::VectorXd q_dot_cmd = svd.solve(T_WE_E_cmd);
  *q_commanded = q + q_dot_cmd * control_period_s_;
  *v_commanded = q_dot_cmd; // This is ignored when constructing iiwa_command.

  bool unsafe_command = Eigen::isnan(q_commanded->array()).any();
  if (unsafe_command) {
    std::cout << "\n\nunsafe command caught inside Step()" << std::endl;
    std::cout << "q_commanded:\n" << *q_commanded << std::endl;
    std::cout << "q_dot_cmd:\n" << q_dot_cmd << std::endl;
    std::cout << "q:\n" << q << std::endl;

    std::cout << "\nT_WE_E_cmd:\n" << T_WE_E_cmd << std::endl;
  }

  bool debug = false;
  if (debug) {
    std::cout << "\n-------\n";
    std::cout << "\nT_WE_E_cmd:\n" << T_WE_E_cmd << std::endl;

    Eigen::Isometry3d H_ErE = H_EEr.inverse();
    std::cout << "H_ErE.translation() " << H_ErE.translation() << std::endl;
  }
  // debugging prints for when things are nan . . .
}
} // namespace robot_plan_runner
} // namespace drake


// Run like this
//./prediction --dataL competition/S1a/dataset6_training.csv --dataT competition/S1b/dataset6_testing.csv --sigma=1.14683 --length=0.0825932 --nu=0.600677 --tau=3.49e-14 --ldl
// Developed by Alexander Litvinenko (RWTH Aachen) and Ronald Kriemann (MIS MPG Leipzig)
// Based on the HLIBPro library (v. 2.9) www.hlibpro.com
// No warranties.

#include <iostream>
#include <fstream>
#include <string>

#include <boost/format.hpp>
#include <boost/program_options.hpp>

#include <gsl/gsl_multimin.h>

#include "hlib.hh"

using namespace std;
using boost::format;
using namespace HLIB;
using namespace boost::program_options;
using  real_t    = HLIB::real;

enum {
    IDX_SIGMA  = 0,
    IDX_LENGTH = 1,
    IDX_NU     = 2,
    IDX_TAU    = 3
};

// global options
int        nmin      = CFG::Cluster::nmin;
double     eps       = 1e-6;
double     fac_eps   = 1e-6;
double     shift     = 1e-7;
bool       use_ldl   = false;

//
// read dataset from file
//
void
read_data ( const std::string &       datafile,
            std::vector< T2Point > &  vertices,
            BLAS::Vector< double > &  Z_data_ )
{
    std::ifstream  in( datafile );

    if ( ! in ) // error
        exit( 1 );

    size_t  N_vtx = 0;

    #if 1

    std::string  line;

    std::getline( in, line );

    if ( line == "x,y,values" )
    {
        std::list< T2Point >  pos;
        std::list< double >   vals;

        while ( std::getline( in, line ) )
        {
            auto    parts = split( line, "," );
            double  x = atof( parts[0].c_str() );
            double  y = atof( parts[1].c_str() );
            double  v = atof( parts[2].c_str() );

            pos.push_back( T2Point( x, y ) );
            vals.push_back( v );
        }// while

        N_vtx = pos.size();

        std::cout << "learning dataset" << std::endl;
        std::cout << N_vtx << std::endl;

        vertices.resize( N_vtx );
        Z_data_ = BLAS::Vector< double >( N_vtx );

        int  i = 0;

        for ( auto  p : pos )
            vertices[ i++ ] = p;

        i = 0;
        int j=0;
        for ( auto  v : vals )
        {
          Z_data_( i++ ) = v;
        }

   }// if
    if ( line == "x,y,ignor" )
    {
        std::list< T2Point >  pos;
        std::list< double >   vals;

        while ( std::getline( in, line ) )
        {
            auto    parts = split( line, "," );
            double  x = atof( parts[0].c_str() );
            double  y = atof( parts[1].c_str() );
            double  v = atof( parts[2].c_str() );

            pos.push_back( T2Point( x, y ) );
           // vals.push_back( v );
        }// while

        N_vtx = pos.size();

        std::cout << "learning dataset" << std::endl;
        std::cout << N_vtx << std::endl;

        vertices.resize( N_vtx );
        Z_data_ = BLAS::Vector< double >( N_vtx );

        int  i = 0;

        for ( auto  p : pos )
            vertices[ i++ ] = p;

        i = 0;

        for ( auto  v : vals )
           Z_data_( i++ ) = 0.0;
    }// if

    if ( line == "x,y" )
    {
        std::list< T2Point >  pos;

        while ( std::getline( in, line ) )
        {
            auto    parts = split( line, "," );
            double  x = atof( parts[0].c_str() );
            double  y = atof( parts[1].c_str() );

            pos.push_back( T2Point( x, y ) );
        }// while

        N_vtx = pos.size();
        std::cout << "testing dataset" << std::endl;
        std::cout << N_vtx << std::endl;

        vertices.resize( N_vtx );

        int  i = 0;

        for ( auto  p : pos )
            vertices[ i++ ] = p;
    }// if
    //else
    //{
    //    std::cout << "you should not be here" << std::endl;
    //    HERROR( ERR_NOT_IMPL, "", "" );
    //}

    #else

    in >> N_vtx;

    std::cout << "reading " << N_vtx << " datapoints" << std::endl;

    vertices.resize( N_vtx );
    Z_data_ = BLAS::Vector< double >( N_vtx );

    for ( idx_t  i = 0; i < idx_t(N_vtx); ++i )
    {
        int     index = i;
        double  x, y, z;
        // double  v     = 0.0;

        in >> index >> x >> y >> z;
        // in >> index >> x >> y >> z >> v;

        vertices[ index ] = T2Point( x, y );
        //Z_data( index )   = v;
    }// for

    #endif

    //
    // for visualization of data, export 2D points with v value in csv file
    //

    // std::ofstream  out( "data.csv" );

    // out << "x,y,z,v" << std::endl;
    // out << "x,y,z" << std::endl;

    //for ( uint  i = 0; i < N_vtx; ++i )
    //   out << vertices[i].x() << "," << vertices[i].y() << ",0" << std::endl;
//      out << vertices[i].x() << "," << vertices[i].y() << ",0," << Z_data( i ) << std::endl;
}

//
// define PredictionProblem to forecast unknown values in new locations
//
struct PredictionProblem
{
    std::vector< T2Point >                vertices;
    std::vector< T2Point >                vertices_predict;
    std::unique_ptr< TCoordinate >        coord;
    std::unique_ptr< TCoordinate >        coord_predict;
    std::unique_ptr< TClusterTree >       ct;
    std::unique_ptr< TClusterTree >       ct_predict;
    std::unique_ptr< TBlockClusterTree >  bct;
    std::unique_ptr< TBlockClusterTree >  bct_predict;
    std::unique_ptr< TVector >            Z;
    std::unique_ptr< TVector >            Z_predict;

    PredictionProblem ( const std::string &  datafile, const std::string &  datafile_predict )
    {
        init( datafile, datafile_predict );
    }

    void
    init ( const std::string &  datafile, const std::string &  datafile_predict )
    {
        BLAS::Vector< double >  Z_data;
        BLAS::Vector< double >  Z_data_predict;

        read_data( datafile, vertices, Z_data );
        read_data( datafile_predict, vertices_predict, Z_data_predict );

        std::cout << "both datasets are succesfully read" << std::endl;
        coord = std::make_unique< TCoordinate >( vertices );
        coord_predict = std::make_unique< TCoordinate >( vertices_predict );

        TAutoBSPPartStrat  part_strat;
        TBSPCTBuilder      ct_builder( & part_strat, nmin );

        ct = ct_builder.build( coord.get() );

        //print_vtk( & coord, "ct_coord" );
        ct_predict = ct_builder.build( coord_predict.get() );
        //print_vtk( & coord_predict, "ct_coord_predict" );

        TStdGeomAdmCond    adm_cond( 2.0, use_min_diam );
        TBCBuilder         bct_builder;

        bct = bct_builder.build( ct.get(), ct.get(), & adm_cond );
        bct_predict = bct_builder.build( ct_predict.get(), ct.get(), & adm_cond );

        Z   = std::make_unique< TScalarVector >( *ct->root(), std::move( Z_data ) );
        Z_predict   = std::make_unique< TScalarVector >( *ct_predict->root(), std::move( Z_data_predict ) );
        ct->perm_e2i()->permute( Z.get() );
    }

    //BLAS::Vector< double >
    std::unique_ptr< TVector >
    eval ( const double  sigma,
           const double  length,
           const double  nu,
           const double tau )
    {

        TMaternCovCoeffFn< T2Point >  matern_coefffn( sigma, length, nu,  vertices );

        TMaternCovCoeffFn< T2Point >  matern_coefffn_predict( sigma, length, nu, vertices_predict, vertices );
        TPermCoeffFn< double >        coefffn( & matern_coefffn, ct->perm_i2e(), ct->perm_i2e() );
        TPermCoeffFn< double >        coefffn_predict( & matern_coefffn_predict, ct_predict->perm_i2e(), ct->perm_i2e() );

        TACAPlus< double >            aca( & coefffn );
        TACAPlus< double >            aca_predict( & coefffn_predict );
        auto                          acc = fixed_prec( eps );
        TDenseMatBuilder< double >    h_builder( & coefffn, & aca );
        TDenseMatBuilder< double >    h_builder_predict( & coefffn_predict, & aca_predict );

        auto                          C        = h_builder.build( bct.get(), acc );
        //TPSMatrixVis  mvis;

        //mvis.svd(true).print( C.get(), "myC" ); Output matrix C as a .eps file

        auto   C_predict= h_builder_predict.build( bct_predict.get(), unsymmetric, acc ); //A_21 - rectangular matrix

        if ( shift != 0.0 )
            add_identity( C.get(), tau*tau );

        auto                          fac_acc  = fixed_prec( fac_eps );
        auto                          C_fac    = C->copy();
        auto                          fac_opts = fac_options_t{ point_wise, CFG::Arith::storage_type, false };

        if ( use_ldl )
        {
            ldl( C_fac.get(), fac_acc, fac_opts );
            //std::cout <<  "LDL is used " << std::endl;
        }
        else
        {
            chol( C_fac.get(), fac_acc );
            //std::cout <<  "Cholesky is used " << std::endl;
        }
        //std::cout << "    |С|_F             = " << norm_F( C.get() ) << std::endl;
        //std::cout << "    |С|_2             = " << norm_2( C.get() ) << std::endl;
        //std::cout << "    |С_fac|_F             = " << norm_F( C_fac.get() ) << std::endl;
        //std::cout << "    |С_fac|_2             = " << norm_2( C_fac.get() ) << std::endl;

        std::unique_ptr< TFacInvMatrix >   C_inv;

        if ( use_ldl )
        {
            C_inv = std::make_unique< TLDLInvMatrix >( C_fac.get(), symmetric, point_wise );
        }
        else
        {
            C_inv = std::make_unique< TLLInvMatrix >( C_fac.get(), symmetric );
        }
        //std::cout << "    |С_inv|_2             = " << norm_2( C_inv.get() ) << std::endl;

        const size_t                  N     = vertices.size();

        TStopCriterion                sstop( 350, 1e-16, 0.0 );
        TCG                           solver( sstop );
        auto                          sol = C->row_vector();

        auto                          Z_predict = C_predict->row_vector();

        FILE* f1;

        solver.solve( C.get(), sol.get(), Z.get(), C_inv.get() );

        //DBG::write( C_predict.get(), "A12.mat", "A12" );
        //std::cout << "  ||invC_Z|| = " << sol->norm2() << std::endl;
        mul_vec( real_t(1), C_predict.get(), sol.get(), real_t(0), Z_predict.get(), apply_normal );

        //std::cout << "  ||Z_predict|| = " << Z_predict->norm2() << std::endl;
        //C_predict->mul_vec( 1.0, sol.get(), 0.0, Z_predict.get(), apply_normal );

        ct_predict->perm_i2e()->permute( Z_predict.get() );
        //TMatlabVectorIO  vio;
        //vio.write( Z_predict,  "x.mat", "x" );

        f1 = fopen("111prediction.txt", "w");
        for ( size_t  i = 0; i < Z_predict->size(); i++ )
          fprintf(f1," %.6f, %.6f, %.6f\n",   vertices_predict[i].x(),  vertices_predict[i].y(), Z_predict->entry(i));
        fclose(f1);
        return std::move( Z_predict );
    }
};

//
// wrapper from GSL to LogLikeliHoodProblem
//
/*double
  eval_logli ( const gsl_vector *  param,
  void *              data )
  {
  double sigma  = gsl_vector_get( param, IDX_SIGMA );
  double length = gsl_vector_get( param, IDX_LENGTH );
  double nu     = gsl_vector_get( param, IDX_NU );
  double tau    = gsl_vector_get( param, IDX_TAU );

  LogLikeliHoodProblem *  problem = static_cast< LogLikeliHoodProblem * >( data );

  return - problem->eval( sigma, length, nu, tau );
  }
*/
//
// optimization function using GSL
//

//
// main function
//
int
main ( int      argc,
       char **  argv )
{

    //CFG::set_verbosity( 3 );
    INIT();

    //std::string  datafile = "datafile.txt";
    //std::string  datafile_predict = "datafile_predict.txt";
    std::string  datafile = "LearningSet.txt";
    std::string  datafile_predict = "TestingSet.txt";

    double  sigma  = 2.7779; //take these values from previous experiments (Part 1a)
    double  length = 0.07;
    double  nu     = 1.0365;
    double  tau    = 4.2045e-8;

    //
    // define command line options
    //

    options_description             all_opts;
    options_description             vis_opts( "usage: loglikelihood [options] datafile\n  where options include" );
    options_description             hid_opts( "Hidden options" );
    positional_options_description  pos_opts;
    variables_map                   vm;

    // standard options
    vis_opts.add_options()
        ( "help,h",                       ": print this help text" )
        ( "threads,t",   value<int>(),    ": number of parallel threads" )
        ( "verbosity,v", value<int>(),    ": verbosity level" )
        ( "nmin",        value<int>(),    ": set minimal cluster size" )
        ( "eps,e",       value<double>(), ": set H accuracy" )
        ( "epslu",       value<double>(), ": set only H factorization accuracy" )
        ( "shift",       value<double>(), ": regularization parameter" )
        ( "ldl",                          ": use LDL factorization" )
        ( "sigma",       value<double>(), ": sigma parameter" )
        ( "nu",          value<double>(), ": nu parameter" )
        ( "length",      value<double>(), ": length parameter" )
        ( "tau",         value<double>(), ": tau paramater" )
        ;

    hid_opts.add_options()
        ( "dataL",        value<std::string>(), ": datafile defining learning problem" )
        ( "dataT",        value<std::string>(), ": datafile defining testing problem" )
        ;

    // options for command line parsing
    all_opts.add( vis_opts ).add( hid_opts );

    // all "non-option" arguments should be "--data" arguments
    pos_opts.add( "dataL", -1 );
    pos_opts.add( "dataT", -1 );

    //
    // parse command line options
    //

    try
    {
        store( command_line_parser( argc, argv ).options( all_opts ).positional( pos_opts ).run(), vm );
        notify( vm );
    }// try
    catch ( required_option &  e )
    {
        std::cout << e.get_option_name() << " requires an argument, try \"-h\"" << std::endl;
        exit( 1 );
    }// catch
    catch ( unknown_option &  e )
    {
        std::cout << e.what() << ", try \"-h\"" << std::endl;
        exit( 1 );
    }// catch

    //
    // eval command line options
    //

    if ( vm.count( "help") )
    {
        std::cout << vis_opts << std::endl;
        exit( 1 );
    }// if

    if ( vm.count( "nmin"      ) ) nmin     = vm["nmin"].as<int>();
    if ( vm.count( "eps"       ) ) eps      = vm["eps"].as<double>();
    if ( vm.count( "epslu"     ) ) fac_eps  = vm["epslu"].as<double>();
    if ( vm.count( "shift"     ) ) shift    = vm["shift"].as<double>();
    if ( vm.count( "threads"   ) ) CFG::set_nthreads( vm["threads"].as<int>() );
    if ( vm.count( "verbosity" ) ) CFG::set_verbosity( vm["verbosity"].as<int>() );
    if ( vm.count( "ldl"       ) ) use_ldl  = true;
    if ( vm.count( "sigma"     ) ) sigma    = vm["sigma"].as<double>();
    if ( vm.count( "nu"        ) ) nu       = vm["nu"].as<double>();
    if ( vm.count( "length"    ) ) length   = vm["length"].as<double>();
    if ( vm.count( "tau"       ) ) tau      = vm["tau"].as<double>();

    // default to general eps
    if ( fac_eps == -1 )
        fac_eps = eps;

    if ( vm.count( "dataL" ) )
        datafile = vm["dataL"].as<std::string>();
    else
    {
        std::cout << "usage: loglikelihood [options] datafile" << std::endl;
        exit( 1 );
    }// if

    if ( vm.count( "dataT" ) )
        datafile_predict = vm["dataT"].as<std::string>();
    else
    {
        std::cout << "usage: loglikelihood [options] datafile" << std::endl;
        exit( 1 );
    }// if

    std::cout << "parameters : " << " σ = " << sigma << ", ℓ = " << length << ", ν = " << nu << ", τ = " << tau << std::endl;

    PredictionProblem  problem( datafile, datafile_predict );
    problem.eval( sigma, length, nu, tau);

    DONE();
}