Matrix3x4.h

Go to the documentation of this file.

/*  Copyright (C) 2003-2021 Free Electron Organization
    Any use of this software requires a license.  If a valid license
    was not distributed with this file, visit freeelectron.org. */

/** @file */

#ifndef __math_Matrix3x4_h__
#define __math_Matrix3x4_h__

namespace fe
{

/**************************************************************************//**
    \brief Matrix for 3D transformations

    @ingroup math

    \verbatim
    Indexes

        0   3   6   9
        1   4   7   10
        2   5   8   11

    represent

        Rot Rot Rot Tx
        Rot Rot Rot Ty
        Rot Rot Rot Tz

    with the columns

        Direction Left Up Translation

    The operator(i,j) addressing for a 4x4 matrix is:
        00  01  02  03
        10  11  12  13
        20  21  22  23
    \endverbatim

    For translation on the right as shown, this is stored column major.
    Note that a few references place translation on the bottom,
    so this would act like row major.

*//***************************************************************************/
template <typename T>
class Matrix<3,4,T>
{
    public:
                        Matrix<3,4,T>(void)
                        {
#if FE_VEC_CHECK_VALID
//                          memset(this,0,sizeof(*this));
                            set(column(0), 0.0f, 0.0f, 0.0f);
                            set(column(1), 0.0f, 0.0f, 0.0f);
                            set(column(2), 0.0f, 0.0f, 0.0f);
                            set(column(3), 0.0f, 0.0f, 0.0f);
#endif
                        }
                        Matrix<3,4,T>(const Matrix<3,4,T>& other)
                        {
                            operator=(other);
                        }
        template<int M,int N,class U>
                        Matrix<3,4,T>(const Matrix<M,N,U>& other)
                        {
                            operator=(other);
                        }
                        Matrix<3,4,T>(const Quaternion<T>& quat)
                        {
                            operator=(quat);
                        }

        Matrix<3,4,T>&  operator=(const Matrix<3,4,T>& other)
                        {
                            if(this != &other)
                            {
                                direction()=other.direction();
                                left()=other.left();
                                up()=other.up();
                                translation()=other.translation();
                            }
                            return *this;
                        }

        template<int M,int N,class U>
        Matrix<3,4,T>&  operator=(const Matrix<M,N,U> &other)
                        {
                            direction()=other.column(0);
                            left()=other.column(1);
                            up()=other.column(2);
                            translation()=other.column(3);
                            return *this;
                        }

        bool            operator==(const Matrix<3,4,T>& other) const;
        bool            operator!=(const Matrix<3,4,T>& other) const
                        {   return !operator==(other); }

        Matrix<3,4,T>&  operator=(const Quaternion<T>& quat);

        Matrix<3,4,T>&  operator*=(const Matrix<3,4,T>& preMatrix);

                        /** @brief Populate a 16 element one dimensional array
                            suitable for OpenGL operations

                            Translation is placed in elements 13, 14, and 15.*/
                        template<class U>
        void            copy16(U* array) const;

                        /** @brief Populate a three element array with
                            the current translation component */
                        template<class U>
        void            copyTranslation(U* array) const;

        Vector<3,T>     column(U32 index) const
                        {   return m_vector[index]; }
        Vector<3,T>&    column(U32 index)
                        {   return m_vector[index]; }

        Vector<3,T>     direction(void) const
                        {   return m_vector[0]; }
        Vector<3,T>     left(void) const
                        {   return m_vector[1]; }
        Vector<3,T>     up(void) const
                        {   return m_vector[2]; }
        Vector<3,T>     translation(void) const
                        {   return m_vector[3]; }

        Vector<3,T>&    direction(void)
                        {   return m_vector[0]; }
        Vector<3,T>&    left(void)
                        {   return m_vector[1]; }
        Vector<3,T>&    up(void)
                        {   return m_vector[2]; }
        Vector<3,T>&    translation(void)
                        {   return m_vector[3]; }

                        /// Column Major addressing
        T               operator()(unsigned int i, unsigned int j) const
                        {   return m_vector[j][i]; }
                        /// Column Major addressing
        T&              operator()(unsigned int i, unsigned int j)
                        {   return m_vector[j][i]; }

static
const   Matrix<3,4,T>   identity(void);

                        // undefined
//      T               operator[](unsigned int index) const;
//      T&              operator[](unsigned int index);

    private:

        Vector<3,T>     m_vector[4];

};

template<class T>
inline bool Matrix<3,4,T>::operator==(const Matrix<3,4,T>& other) const
{
    return (0 == memcmp(reinterpret_cast<const void *>(m_vector),
            reinterpret_cast<const void *>(other.m_vector),
            4*sizeof(Vector<3,T>)));
};

/** Equivalence test within the given tolerance @em margin
    @relates Vector
    */
template<class T,class U>
inline bool equivalent(const Matrix<3,4,T>& lhs,
    const Matrix<3,4,T>& rhs, U margin)
{
    return (equivalent(lhs.direction(), rhs.direction(),    margin) &&
            equivalent(lhs.left(),          rhs.left(),         margin) &&
            equivalent(lhs.up(),            rhs.up(),           margin) &&
            equivalent(lhs.translation(),   rhs.translation(),  margin));
}

/** @brief Copy from array of 16 values

    @relates Matrix3x4

    Note that the array is presumed to be in a standard OpenGL 4x4 layout.
    Since Matrix3x4 retains 12 values, 4 of the input values will be ignored.
    */
template<class T,class U>
inline Matrix<3,4,T>& set(Matrix<3,4,T>& lhs,const U* pArray)
{
    set(lhs.column(0), pArray[0], pArray[1], pArray[2]);
    set(lhs.column(1), pArray[4], pArray[5], pArray[6]);
    set(lhs.column(2), pArray[8], pArray[9], pArray[10]);
    set(lhs.column(3), pArray[12], pArray[13], pArray[14]);
    return lhs;
}

/** @brief Set as the rotation between two vectors

    @relates Matrix3x4

    Operands should be pre-normalized.

    Twist should be considered arbitrary.
    */
template<class T>
inline Matrix<3,4,T>& set(Matrix<3,4,T>& lhs,const Vector<3,T>& from,
        const Vector<3,T>& to)
{
    if(isZero(from) || isZero(to))
    {
        feX("Matrix<3,4,T>:set (from,to)","zero length vector");
    }

    F32 cosa=dot(from,to);
    if(cosa>FE_ALMOST1)
    {
        setIdentity(lhs);
        return lhs;
    }

    Vector<3,T> axis;
    F32 sina;
    if(cosa< -FE_ALMOST1)
    {
        cosa= -1.0;
        sina= 0.0;
        set(axis,0.0,from[0],-from[1]);

        T len=sqrt(axis[1]*axis[1] + axis[2]*axis[2]);
        if (len < FE_SLERP_DELTA)
        {
            set(axis,-from[2],0.0,from[0]);
        }
    }
    else
    {
        sina=sin(acos(cosa));
        axis=cross(from,to);
    }

    normalize(axis);

    const F32 mcosa=1-cosa;

    set(lhs.column(0),
            axis[0]*axis[0]*mcosa + cosa,
            axis[1]*axis[0]*mcosa + axis[2]*sina,
            axis[2]*axis[0]*mcosa - axis[1]*sina);

    set(lhs.column(1),
            axis[0]*axis[1]*mcosa - axis[2]*sina,
            axis[1]*axis[1]*mcosa + cosa,
            axis[2]*axis[1]*mcosa + axis[0]*sina);

    set(lhs.column(2),
            axis[0]*axis[2]*mcosa + axis[1]*sina,
            axis[1]*axis[2]*mcosa - axis[0]*sina,
            axis[2]*axis[2]*mcosa + cosa);

    set(lhs.column(3), 0.0f, 0.0f, 0.0f);

    return lhs;
}

template <typename T>
inline Matrix<3,4,T>& setIdentity(Matrix<3,4,T>& lhs)
{
    set(lhs.column(0), 1.0f, 0.0f, 0.0f);
    set(lhs.column(1), 0.0f, 1.0f, 0.0f);
    set(lhs.column(2), 0.0f, 0.0f, 1.0f);
    set(lhs.column(3), 0.0f, 0.0f, 0.0f);
    return lhs;
}

/** @brief Set the rotation of the transform

    @relates Matrix3x4

    Sets the first three columns of the transform equal to
    a rotation matrix (3x3 / SpatialMatrix)
    */
template <typename T>
inline Matrix<3,4,T>& setRotation(Matrix<3,4,T>& lhs,
        const Matrix<3,3,T>& rotation)
{
    set(lhs.column(0), rotation(0, 0), rotation(1, 0), rotation(2, 0));
    set(lhs.column(1), rotation(0, 1), rotation(1, 1), rotation(2, 1));
    set(lhs.column(2), rotation(0, 2), rotation(1, 2), rotation(2, 2));
    return lhs;
}

/** @brief Set the translation of the transform

    @relates Matrix3x4

    Sets the fourth column of the transform, which represents position,
    equal to the supplied vector
    */
template <int N, typename T>
inline Matrix<3,4,T>& setTranslation(Matrix<3,4,T>& lhs,
        const Vector<N,T>& translation)
{
    FEASSERT(N>2);
    lhs.translation()=translation;

    return lhs;
}

/** @brief Apply transform's rotation to a vector

    @relates Matrix3x4

    Rotates the supplied vector by the rotation matrix in the transform.
    */
template <int N, typename T>
inline void rotateVector(const Matrix<3,4,T>& lhs,const Vector<N,T>& in,
        Vector<N,T>& out)
{
    FEASSERT(N>2);
    fe::set(out,in[0]*lhs(0,0)+in[1]*lhs(0,1)+in[2]*lhs(0,2),
                in[0]*lhs(1,0)+in[1]*lhs(1,1)+in[2]*lhs(1,2),
                in[0]*lhs(2,0)+in[1]*lhs(2,1)+in[2]*lhs(2,2));
}

template <int N, typename T>
inline Vector<N,T> rotateVector(const Matrix<3,4,T>& lhs,const Vector<N,T>& in)
{
    FEASSERT(N>2);
    Vector<N,T> out;
    rotateVector(lhs,in,out);
    return out;
}

template <int N, typename T>
inline Vector<N,T> inverseRotateVector(const Matrix<3,4,T>& lhs,
        const Vector<N,T>& in)
{
    FEASSERT(N>2);
    Matrix<3,4,T> inverse;
    invert(inverse,lhs);
    return rotateVector(inverse,in);
}

/** @brief Transform the vector

    @relates Matrix3x4

    Applies the transform's rotation, then add the transform's translation.
    */
template <int N, typename T>
inline void transformVector(const Matrix<3,4,T>& lhs,const Vector<N,T>& in,
        Vector<N,T>& out)
{
    FEASSERT(N>2);
    fe::set(out,in[0]*lhs(0,0)+in[1]*lhs(0,1)+in[2]*lhs(0,2)+lhs(0,3),
                in[0]*lhs(1,0)+in[1]*lhs(1,1)+in[2]*lhs(1,2)+lhs(1,3),
                in[0]*lhs(2,0)+in[1]*lhs(2,1)+in[2]*lhs(2,2)+lhs(2,3));
}

template <int N, typename T>
inline Vector<N,T> transformVector(const Matrix<3,4,T>& lhs,
        const Vector<N,T>& in)
{
    FEASSERT(N>2);
    Vector<N,T> out;
    transformVector(lhs,in,out);
    return out;
}

template <int N, typename T>
inline Vector<N,T> inverseTransformVector(const Matrix<3,4,T>& lhs,
        const Vector<N,T>& in)
{
    FEASSERT(N>2);
    Matrix<3,4,T> inverse;
    invert(inverse,lhs);
    return transformVector(inverse,in);
}

/** @brief Translate the transform

    @relates Matrix3x4

    Rotates the vector by the transform's rotation, then adds
    to the transform's translation.
    */
template <int N, typename T>
inline Matrix<3,4,T>& translate(Matrix<3,4,T>& lhs,
        const Vector<N,T>& translation)
{
    FEASSERT(N>2);
    Vector<N,T> rotated;
    rotateVector(lhs,translation,rotated);

    lhs.translation()+=rotated;
    return lhs;
}

template <typename T>
inline void setColumn(int i, Matrix<3,4,T>& lhs, const Vector<3,T>& v)
{
    lhs.column(i)=v;
}

template <typename T>
inline void getColumn(int i, const Matrix<3,4,T>& lhs, Vector<3,T>& v)
{
    v=lhs.column(i);
}

/** @brief Rotate the transform

    @relates Matrix3x4

    Rotates the transform ccw about the specified axis.
    */
template <typename T,typename U>
inline Matrix<3,4,T>& rotate(Matrix<3,4,T>& lhs,U radians,Axis axis)
{
    const T sina=sin(radians);
    const T cosa=cos(radians);

    static const U32 index[3][2]={{1,2},{2,0},{0,1}};
    U32 x1=index[axis][0];
    U32 x2=index[axis][1];

    T b=lhs(0,x1);
    T c=lhs(0,x2);
    lhs(0,x1)=c*sina+b*cosa;
    lhs(0,x2)=c*cosa-b*sina;
    b=lhs(1,x1);
    c=lhs(1,x2);
    lhs(1,x1)=c*sina+b*cosa;
    lhs(1,x2)=c*cosa-b*sina;
    b=lhs(2,x1);
    c=lhs(2,x2);
    lhs(2,x1)=c*sina+b*cosa;
    lhs(2,x2)=c*cosa-b*sina;
/*
    static const U32 index[3][2]={{3,6},{6,0},{0,3}};
    U32 x1=index[axis][0];
    U32 x2=index[axis][1];

#if FALSE
    U32 y;
    for(y=0;y<3;y++)
    {
        T b=lhs[x1];
        T c=lhs[x2];

        lhs[x1++]=c*sina+b*cosa;
        lhs[x2++]=c*cosa-b*sina;
    }
#else
    T b=lhs[x1];
    T c=lhs[x2];
    lhs[x1++]=c*sina+b*cosa;
    lhs[x2++]=c*cosa-b*sina;
    b=lhs[x1];
    c=lhs[x2];
    lhs[x1++]=c*sina+b*cosa;
    lhs[x2++]=c*cosa-b*sina;
    b=lhs[x1];
    c=lhs[x2];
    lhs[x1++]=c*sina+b*cosa;
    lhs[x2++]=c*cosa-b*sina;
#endif
*/

    return lhs;
}

// cannot overload this as 'scale' since compiler has trouble matching
// Vector<N,U> derived classes to the other scales when this function exists
// as 'scale'.
template <typename T,typename U>
inline Matrix<3,4,T>& scaleAll(Matrix<3,4,T>& lhs,U scalefactor)
{
    lhs(0,0)*=scalefactor;
    lhs(1,1)*=scalefactor;
    lhs(2,2)*=scalefactor;
    return lhs;
}

template <typename T,typename U,int N>
inline Matrix<3,4,T>& scale(Matrix<3,4,T>& lhs,Vector<N,U> scalefactor)
{
    lhs(0,0)*=scalefactor[0];
    lhs(1,1)*=scalefactor[1];
    lhs(2,2)*=scalefactor[2];
    return lhs;
}

template <typename T,typename U>
inline Matrix<3,4,T>& scale(Matrix<3,4,T>& lhs,Vector<2,U> scalefactor)
{
    lhs(0,0)*=scalefactor[0];
    lhs(1,1)*=scalefactor[1];
    return lhs;
}

template <typename T,typename U>
inline Matrix<3,4,T>& scale(Matrix<3,4,T>& lhs,Vector<1,U> scalefactor)
{
    lhs(0,0)*=scalefactor[0];
    return lhs;
}

template <typename T>
template <typename U>
inline void Matrix<3,4,T>::copy16(U* array) const
{
    array[0]=m_vector[0][0];
    array[1]=m_vector[0][1];
    array[2]=m_vector[0][2];
    array[3]=0.0f;

    array[4]=m_vector[1][0];
    array[5]=m_vector[1][1];
    array[6]=m_vector[1][2];
    array[7]=0.0f;

    array[8]=m_vector[2][0];
    array[9]=m_vector[2][1];
    array[10]=m_vector[2][2];
    array[11]=0.0f;

    array[12]=m_vector[3][0];
    array[13]=m_vector[3][1];
    array[14]=m_vector[3][2];
    array[15]=1.0f;
}

template<int M, int N, class T>
inline void copy(Matrix<3,4,T>& lhs,const Matrix<M,N,T>& rhs)
{
    setIdentity(lhs);
    int m;
    if(M < 3) { m = M; }
    else { m = 3; }
    int n;
    if(N < 4) { n = N; }
    else { n = 4; }

    for(int i = 0;i < m; i++)
    {
        for(int j = 0;j < n; j++)
        {
            lhs(i,j) = rhs(i,j);
        }
        for(int j = n;j < 4; j++)
        {
            lhs(i,j) = T(0);
        }
    }
    for(int i = m;i < 3; i++)
    {
        for(int j = 0;j < 4; j++)
        {
            lhs(i,j) = T(0);
        }
    }
}

template<int M, int N, class T>
inline void copy(Matrix<M,N,T>& lhs,const Matrix<3,4,T>& rhs)
{
    setIdentity(lhs);

    int m;
    if(M < 3) { m = M; }
    else { m = 3; }
    int n;
    if(N < 4) { n = N; }
    else { n = 4; }

    for(int i = 0;i < m; i++)
    {
        for(int j = 0;j < n; j++)
        {
            lhs(i,j) = rhs(i,j);
        }
        for(int j = n;j < N; j++)
        {
            lhs(i,j) = T(0);
        }
    }
    for(int i = m;i < M; i++)
    {
        for(int j = 0;j < N; j++)
        {
            lhs(i,j) = T(0);
        }
    }
}

template <typename T>
template <typename U>
inline void Matrix<3,4,T>::copyTranslation(U* array) const
{
    array[0]=m_vector[3][0];
    array[1]=m_vector[3][1];
    array[2]=m_vector[3][2];
}

template <typename T>
inline const Matrix<3,4,T> Matrix<3,4,T>::identity(void)
{
    Matrix<3,4,T> matrix;
    setIdentity(matrix);
    return matrix;
}

template <typename T>
inline Matrix<3,4,T>& Matrix<3,4,T>::operator=(const Quaternion<T>& quat)
{
    const T x2=quat[0]+quat[0];
    const T y2=quat[1]+quat[1];
    const T z2=quat[2]+quat[2];
    const T xx=quat[0]*x2;
    const T xy=quat[0]*y2;
    const T xz=quat[0]*z2;
    const T yy=quat[1]*y2;
    const T yz=quat[1]*z2;
    const T zz=quat[2]*z2;
    const T wx=quat[3]*x2;
    const T wy=quat[3]*y2;
    const T wz=quat[3]*z2;

    // this is the transpose of what the reference said
    set(m_vector[0], 1.0f-(yy+zz), xy+wz, xz-wy);
    set(m_vector[1], xy-wz, 1.0f-(xx+zz), yz+wx);
    set(m_vector[2], xz+wy, yz-wx, 1.0f-(xx+yy));
    set(m_vector[3], 0.0f, 0.0f, 0.0f);

    return *this;
}

#if FALSE
/** Matrix Matrix multiply
    @relates Matrix3x4
    */
template<typename T, typename U>
inline Matrix<3,4,T>& multiply(Matrix<3,4,T>& R, const Matrix<3,4,T>& A,
        const Matrix<3,4,U>& B)
{
    R[0] = A[0]*B[0] + A[3]*B[1] + A[6]*B[2] + A[9];
    R[1] = A[1]*B[0] + A[4]*B[1] + A[7]*B[2] + A[10];
    R[2] = A[2]*B[0] + A[5]*B[1] + A[8]*B[2] + A[11];

    R[3] = A[0]*B[3] + A[3]*B[4] + A[6]*B[5] + A[9];
    R[4] = A[1]*B[3] + A[4]*B[4] + A[7]*B[5] + A[10];
    R[5] = A[2]*B[3] + A[5]*B[4] + A[8]*B[5] + A[11];

    R[6] = A[0]*B[6] + A[3]*B[7] + A[6]*B[8] + A[9];
    R[7] = A[1]*B[6] + A[4]*B[7] + A[7]*B[8] + A[10];
    R[8] = A[2]*B[6] + A[5]*B[7] + A[8]*B[8] + A[11];

    R[9] = A[0]*B[9] + A[3]*B[10]+ A[6]*B[11]+ A[9];
    R[10]= A[1]*B[9] + A[4]*B[10]+ A[7]*B[11]+ A[10];
    R[11]= A[2]*B[9] + A[5]*B[10]+ A[8]*B[11]+ A[11];
    return R;
}

/** Matrix Matrix multiply
    @relates Matrix3x4
    */
template<typename T, typename U>
inline Matrix<3,4,T> operator*(const Matrix<3,4,T>& lhs,
        const Matrix<3,4,U>& rhs)
{
    Matrix<3,4,T> C;
    multiply(C,lhs,rhs);
    return C;
}
#endif

/** Matrix Matrix add
    @relates Matrix3x4
    */
template<class T, class U>
inline Matrix<3,4,T>& add(Matrix<3,4,T>& result, const Matrix<3,4,T>& lhs,
        const Matrix<3,4,U>& rhs)
{
    result.column(0) = lhs.column(0) + rhs.column(0);
    result.column(1) = lhs.column(1) + rhs.column(1);
    result.column(2) = lhs.column(2) + rhs.column(2);
    result.column(3) = lhs.column(3) + rhs.column(3);
    return result;
}

/** Matrix Matrix subtract
    @relates Matrix3x4
    */
template<class T, class U>
inline Matrix<3,4,T>& subtract(Matrix<3,4,T>& result, const Matrix<3,4,T>& lhs,
        const Matrix<3,4,U>& rhs)
{
    result.column(0) = lhs.column(0) - rhs.column(0);
    result.column(1) = lhs.column(1) - rhs.column(1);
    result.column(2) = lhs.column(2) - rhs.column(2);
    result.column(3) = lhs.column(3) - rhs.column(3);
    return result;
}

/** Matrix scale
    @relates Matrix3x4
    */
template<class T, class U>
inline Matrix<3,4,T>& multiply(Matrix<3,4,T>& result,
        const U lhs, const Matrix<3,4,T>& rhs)
{
    result.column(0) = lhs * rhs.column(0);
    result.column(1) = lhs * rhs.column(1);
    result.column(2) = lhs * rhs.column(2);
    result.column(3) = lhs * rhs.column(3);
    return result;
}

/** Matrix scale
    @relates Matrix3x4
    */
template<class T, class U>
inline Matrix<3,4,T>& multiply(Matrix<3,4,T>& result,
        const Matrix<3,4,T>& lhs, const U rhs)
{
    result.column(0) = rhs * lhs.column(0);
    result.column(1) = rhs * lhs.column(1);
    result.column(2) = rhs * lhs.column(2);
    result.column(3) = rhs * lhs.column(3);
    return result;
}

/** Matrix Matrix add
    @relates Matrix3x4
    */
template<typename T, typename U>
inline Matrix<3,4,T> operator+(const Matrix<3,4,T>& lhs,
        const Matrix<3,4,U>& rhs)
{
    Matrix<3,4,T> tmp;
    add(tmp,lhs,rhs);
    return tmp;
}

/** Matrix Matrix subtract
    @relates Matrix3x4
    */
template<typename T, typename U>
inline Matrix<3,4,T> operator-(const Matrix<3,4,T>& lhs,
        const Matrix<3,4,U>& rhs)
{
    Matrix<3,4,T> tmp;
    subtract(tmp,lhs,rhs);
    return tmp;
}

/** Matrix scale
    @relates Matrix3x4
    */
template<class T, class U>
inline Matrix<3,4,T> operator*(const U lhs, const Matrix<3,4,T>& rhs)
{
    Matrix<3,4,T> tmp;
    multiply(tmp,lhs,rhs);
    return tmp;
}

/** Matrix scale
    @relates Matrix3x4
    */
template<class T, class U>
inline Matrix<3,4,T> operator*(const Matrix<3,4,T>& lhs, const U rhs)
{
    Matrix<3,4,T> tmp;
    multiply(tmp,lhs,rhs);
    return tmp;
}

template <typename T>
inline Matrix<3,4,T>& Matrix<3,4,T>::operator*=(const Matrix<3,4,T>& preMatrix)
{
    return *this=preMatrix*(*this);
}

/** Return the horizonatal dimension
    @relates Matrix3x4
    */
template<class T>
inline U32 width(const Matrix<3,4,T>& matrix)
{
    return 3;
}

/** Return the vertical dimension
    @relates Matrix3x4
    */
template<class T>
inline U32 height(const Matrix<3,4,T>& matrix)
{
    return 4;
}

/** Matrix print
    @relates Matrix3x4
    */
template<class T>
inline String print(const Matrix<3,4,T>& a_matrix)
{
    String s;
    for(unsigned int i = 0;i < 3; i++)
    {
        for(unsigned int j = 0;j < 4; j++)
        {
            s.catf("%6.3f ", a_matrix(i,j));
        }
        if(i!=2)
        {
            s.cat("\n");
        }
    }
    return s;
}

/** @brief 3x4 matrix inversion wrapped as a 4x4

    @relates Matrix3x4
*/
template <typename T>
inline Matrix<3,4,T>& invert(Matrix<3,4,T>& inverse,
        const Matrix<3,4,T>& matrix)
{
    //* TODO optimize for zeros instead of reusing 4x4

    Matrix<4,4,T> inverse4x4,matrix4x4;

    copy(matrix4x4,matrix);
    matrix4x4(3,3)=1.0f;
    invert(inverse4x4,matrix4x4);
    copy(inverse,inverse4x4);
    return inverse;
}

template <typename T>
inline T determinant(const Matrix<3,4,T>& matrix)
{
    //* TODO optimize for zeros instead of reusing 4x4

    Matrix<4,4,T> matrix4x4;

    copy(matrix4x4,matrix);
    matrix4x4(3,3)=1.0f;
    return determinant(matrix4x4);
}

/** @brief Interpolate between two matrices

    @relates Matrix3x4

    This is a naive linear interpolation that considers the translation
    and rotation separately.
    For an pure interpolation, see MatrixPower in the solve module.
*/
template <typename T,typename U>
inline Matrix<3,4,T>& interpolate(Matrix<3,4,T>& result,
        const Matrix<3,4,T>& matrix1,const Matrix<3,4,T>& matrix2,
        U fraction)
{
    Matrix<3,4,T> matrix1_inv;
    invert(matrix1_inv,matrix1);
    Matrix<3,4,T> delta=matrix2;
    delta*=matrix1_inv;

    Quaternion<T> spin=delta;
    spin*=fraction;
    Matrix<3,4,T> mspin(spin);

    Vector<3,T> move=delta.translation();
    move*=fraction;

    setIdentity(result);
    translate(result,move);
    result=mspin*matrix1*result;

    return result;
}

/** @brief Return the transform to properly place a billboard

    @relates Matrix3x4

    The resulting transform should move a Z-facing object at the origin
    to the given @em center and @direction, doing it's best to keep
    the local x axis parallel to the world XZ plane. */
template <typename T>
inline Matrix<3,4,T> billboardTransform(const Vector<3,T>& center,
        const Vector<3,T>& direction)
{
    Vector<3,T> zAxis(0.0f,0.0f,1.0f);
    Quaternion<T> rotation;

    set(rotation,zAxis,unit(direction));
//  removeTwistAboutX(rotation);
    Matrix<3,4,T> result=rotation;
    setTranslation(result,center);

    return result;
}

template<class T>
inline Matrix<3,4,T> operator*(const Matrix<3,4,T>& left,
        const Matrix<3,4,T>& right)
{
    // [i][j]=sum of [i][k]*[k][j] for all k

    Matrix<3,4,T> matrix;

    set(matrix.column(0),
            left(0,0)*right(0,0)+left(1,0)*right(0,1)+left(2,0)*right(0,2),
            left(0,0)*right(1,0)+left(1,0)*right(1,1)+left(2,0)*right(1,2),
            left(0,0)*right(2,0)+left(1,0)*right(2,1)+left(2,0)*right(2,2));

    set(matrix.column(1),
            left(0,1)*right(0,0)+left(1,1)*right(0,1)+left(2,1)*right(0,2),
            left(0,1)*right(1,0)+left(1,1)*right(1,1)+left(2,1)*right(1,2),
            left(0,1)*right(2,0)+left(1,1)*right(2,1)+left(2,1)*right(2,2));

    set(matrix.column(2),
            left(0,2)*right(0,0)+left(1,2)*right(0,1)+left(2,2)*right(0,2),
            left(0,2)*right(1,0)+left(1,2)*right(1,1)+left(2,2)*right(1,2),
            left(0,2)*right(2,0)+left(1,2)*right(2,1)+left(2,2)*right(2,2));

    set(matrix.column(3),
            left(0,3)*right(0,0)+left(1,3)*right(0,1)+left(2,3)*right(0,2)+
            right(0,3),
            left(0,3)*right(1,0)+left(1,3)*right(1,1)+left(2,3)*right(1,2)+
            right(1,3),
            left(0,3)*right(2,0)+left(1,3)*right(2,1)+left(2,3)*right(2,2)+
            right(2,3));

    return matrix;
}

template<class T>
inline Quaternion<T>::Quaternion(const Matrix<3,4,T>& matrix)
{
    operator=(matrix);
}

//* TODO might be invalid if Matrix is scaled
template<class T>
inline Quaternion<T>& Quaternion<T>::operator=(const Matrix<3,4,T>& matrix)
{
    T tr,s;

    tr=matrix(0,0)+matrix(1,1)+matrix(2,2);

    if(tr>0.0f)
    {
        // diagonal is positive
        s=sqrt(tr+1.0f);
        T s2=0.5f/s;

        set(*this,(matrix(2,1)-matrix(1,2))*s2,(matrix(0,2)-matrix(2,0))*s2,
                (matrix(1,0)-matrix(0,1))*s2,0.5f*s);
    }
    else
    {
        // diagonal is negative
        int i=0;
        if(matrix(1,1)>matrix(0,0))
        {
            i=1;
        }

        if(matrix(2,2)>matrix(i,i))
        {
            i=2;
        }

        const int nxt[3]={1,2,0};
        int j=nxt[i];
        int k=nxt[j];

        s=sqrt((matrix(i,i)-(matrix(j,j)+matrix(k,k)))+1.0f);

        T q[4];

        q[i]=s*0.5f;

        if(s!=0.0f)
            s=0.5f/s;

        q[3]=(matrix(k,j)-matrix(j,k))*s;
        q[j]=(matrix(j,i)+matrix(i,j))*s;
        q[k]=(matrix(k,i)+matrix(i,k))*s;

        set(*this,q[0],q[1],q[2],q[3]);
    }

    normalize(*this);
    return *this;
}


typedef Matrix<3,4,F32> Matrix3x4f;
typedef Matrix<3,4,F64> Matrix3x4d;
typedef Matrix<3,4,Real> SpatialTransform;

FE_DL_PUBLIC BWORD makeFrame(SpatialTransform &a_frame,
        const Vector<3,Real> &a_0, const Vector<3,Real> &a_1,
        const Vector<3,Real> &a_2, const Vector<3,Real> &a_offset);

FE_DL_PUBLIC BWORD makeFrame(SpatialTransform &a_frame,
        const Vector<3,Real> &a_0, const Vector<3,Real> &a_1,
        const Vector<3,Real> &a_2, const SpatialTransform& a_transform);

FE_DL_PUBLIC BWORD makeFrame(SpatialTransform &a_frame,
        const Vector<3,Real> &a_location,
        const Vector<3,Real> &a_direction, const Vector<3,Real> &a_up);

FE_DL_PUBLIC BWORD makeFrameCameraZ(SpatialTransform &a_frame,
    const Vector<3,Real> &a_location,
    const Vector<3,Real> &a_yDir, const Vector<3,Real> &a_cameraZ);

//* NOTE tangentX and normalY are only different by which axis is unchanged
FE_DL_PUBLIC BWORD makeFrameTangentX(SpatialTransform &a_frame,
        const Vector<3,Real> &a_location,
        const Vector<3,Real> &a_tangentX, const Vector<3,Real> &a_normalY);

FE_DL_PUBLIC BWORD makeFrameNormalY(SpatialTransform &a_frame,
        const Vector<3,Real> &a_location,
        const Vector<3,Real> &a_tangentX, const Vector<3,Real> &a_normalY);

FE_DL_PUBLIC BWORD makeFrameNormalZ(SpatialTransform &a_frame,
        const Vector<3,Real> &a_location,
        const Vector<3,Real> &a_tangentX, const Vector<3,Real> &a_normalZ);

} /* namespace */

#endif /* __math_Matrix3x4_h__ */