Matrix.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_matrix_h__
#define __math_matrix_h__

namespace fe
{
/**************************************************************************//**
    @brief General template for fixed size numeric matrices

    @ingroup math

    @verbatim
    Dimensions specified as MxN:

         N
        ____
       |    |
    M  |    |
       |____|

    With storage for a 4x4 as:
        0   4   8   12
        1   5   9   13
        2   6   10  14
        3   7   11  15

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

    This is sometimes called column-major.
    @endverbatim
*//***************************************************************************/
template<int M, int N, class T>
class Matrix
{
    public:
                        Matrix(void)                                        {}
                        Matrix(const Matrix<M,N,T> &other)  { operator=(other);}

        Matrix<M,N,T>&  operator=(const Matrix<M,N,T> &other);
        template<class U>
        Matrix<M,N,T>&  operator=(const Matrix<M,N,U> &other);
        template<int I,int J,class U>
        Matrix<M,N,T>&  operator=(const Matrix<I,J,U> &other);
        bool            operator==(const Matrix<M,N,T> &other) const;

                        /// Column Major addressing
        T               operator()(unsigned int i, unsigned int j) const;
                        /// Column Major addressing
        T&              operator()(unsigned int i, unsigned int j);

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

        Vector<M,T>     column(unsigned int index) const
                        {   return Vector<M,T>(m_data+4*index); }

        T               *raw(void)                          { return m_data; }
const   T               *raw(void) const                    { return m_data; }

    protected:
        T               m_data[M*N];
};


template<int M, int N, class T>
inline Matrix<M,N,T> &Matrix<M,N,T>::operator=(const Matrix<M,N,T> &other)
{
    if(this != &other)
        for(unsigned int i = 0;i < M*N;i++)
            m_data[i] = other[i];
    return *this;
}

template<int M, int N, class T>
template<class U>
inline Matrix<M,N,T> &Matrix<M,N,T>::operator=(const Matrix<M,N,U> &other)
{
    for(unsigned int i = 0;i < M*N;i++)
        m_data[i] = (T)other[i];
    return *this;
}

template<int M, int N, class T>
template<int I,int J,class U>
inline Matrix<M,N,T> &Matrix<M,N,T>::operator=(const Matrix<I,J,U> &other)
{
    copy(*this,other);
    return *this;
}

template<int M, int N, class T>
inline bool Matrix<M,N,T>::operator==(const Matrix<M,N,T> &other) const
{
    return (0 == memcmp(reinterpret_cast<const void *>(m_data),
        reinterpret_cast<const void *>(other.m_data), M*N*sizeof(T)));
}

template<int M, int N, class T>
inline T Matrix<M,N,T>::operator()(unsigned int i, unsigned int j) const
{
#if FE_BOUNDSCHECK
    if(i >= M || j >= N) { feX(e_invalidRange); }
#endif
    return m_data[ j*M + i ];
}

template<int M, int N, class T>
inline T &Matrix<M,N,T>::operator()(unsigned int i, unsigned int j)
{
#if FE_BOUNDSCHECK
    if(i >= M || j >= N) { feX(e_invalidRange); }
#endif
    return m_data[ j*M + i ];
}

template<int M, int N, class T>
inline T Matrix<M,N,T>::operator[](unsigned int index) const
{
#if FE_BOUNDSCHECK
    if(index > M*N) { feX(e_invalidRange); }
#endif
    return m_data[ index ];
}

template<int M, int N, class T>
inline T &Matrix<M,N,T>::operator[](unsigned int index)
{
#if FE_BOUNDSCHECK
    if(index > M*N) { feX(e_invalidRange); }
#endif
    return m_data[ index ];
}

/* non member operations */

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

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

/** Return true is matrix is square, otherwise return false.
    @relates Matrix
    */
template<int M, int N, class T>
inline bool isSquare(const Matrix<M,N,T> &matrix)
{
    return (M==N);
}

/** Set matrix to identity matrix
    @relates Matrix
    */
template<int M, int N, class T>
inline Matrix<M,N,T> &setIdentity(Matrix<M,N,T> &matrix)
{
    set(matrix);
    for(unsigned int i = 0;i < M && i < N;i++)
        matrix(i,i) = 1.0;
    return matrix;
}

template<int M, int N, class T>
inline Matrix<M,N,T>& set(Matrix<M,N,T>& matrix)
{
    return setAll(matrix,T(0));
}

template<int M, int N, class T,class U>
inline Matrix<M,N,T>& set(Matrix<M,N,T>& matrix,const U* pArray)
{
    for(unsigned int i = 0;i < M*N;i++)
        matrix[i] = pArray[i];
    return matrix;
}

template<int M, int N, class T,class U>
inline Matrix<M,N,T>& setAll(Matrix<M,N,T>& matrix,const U value)
{
    for(unsigned int i = 0;i < M*N;i++)
    {
        matrix[i] = value;
    }
    return matrix;
}

/** Return transpose of matrix.
    @relates Matrix
    */
template<int M, int N, class T>
inline Matrix<N,M,T> transpose(const Matrix<M,N,T> &matrix)
{
    Matrix<N,M,T> A;
    for(unsigned int i=0; i<M; i++)
    {
        for(unsigned int j=0; j < N; j++)
        {
            A(j,i) = matrix(i,j);
        }
    }
    return A;
}

/** Transpose matrix in place.
    @relates Matrix
    */
template<int M, int N, class T>
inline Matrix<M,N,T> &setTranspose(Matrix<M,N,T> &matrix)
{
    if(!isSquare(matrix))
    {
        feX("transpose",
            "cannot transpose non-square matrix in place");
    }
    T tmp;
    for(unsigned int i=0; i<M; i++)
    {
        for(unsigned int j=i+1; j < N; j++)
        {
            tmp = matrix(i,j);
            matrix(i,j) = matrix(j,i);
            matrix(j,i) = tmp;
        }
    }
    return matrix;
}

/** Matrix Matrix add
    @relates Matrix
    */
template<int M, int N, class T, class U>
inline Matrix<M,N,T> &add(Matrix<M,N,T> &R, const Matrix<M,N,T> &A,
        const Matrix<M,N,U> &B)
{
    for(unsigned int i = 0;i < M; i++)
    {
        for(unsigned int j = 0;j < N; j++)
        {
            R(i,j) = A(i,j)+B(i,j);
        }
    }
    return R;
}

/** Matrix Matrix subtract
    @relates Matrix
    */
template<int M, int N, class T, class U>
inline Matrix<M,N,T> &subtract(Matrix<M,N,T> &R, const Matrix<M,N,T> &A,
        const Matrix<M,N,U> &B)
{
    for(unsigned int i = 0;i < M; i++)
    {
        for(unsigned int j = 0;j < N; j++)
        {
            R(i,j) = A(i,j)-B(i,j);
        }
    }
    return R;
}

/** Matrix Matrix multiply
    @relates Matrix
    */
template<int M, int N, int L, class T, class U>
inline Matrix<M,N,T> &multiply(Matrix<M,N,T> &R, const Matrix<M,L,T> &A,
        const Matrix<L,N,U> &B)
{
    for(unsigned int i = 0;i < M; i++)
    {
        for(unsigned int j = 0;j < N; j++)
        {
            T sum=T(0);
            for(unsigned int k = 0;k < L; k++)
            {
                sum += A(i,k)*B(k,j);
            }
            R(i,j)=sum;
        }
    }
    return R;
}

/** Matrix Vector multiply
    @relates Vector
    */
template<int M, int N, class T, class U>
inline Vector<M,T> &multiply(Vector<M,T> &b, const Matrix<M,N,T> &A,
        const Vector<N,U> &x)
{
    for(unsigned int i = 0;i < M; i++)
    {
        T sum=T(0);
        for(unsigned int j = 0;j < N; j++)
        {
            sum+=A(i,j) * x[j];
        }
        setAt(b,i,sum);
    }
    return b;
}

/** Matrix Vector multiply
    @relates Vector
    */
template<int M, int N, class T, class U>
inline Vector<M,T> multiply(const Matrix<M,N,T> &A,
        const Vector<N,U> &x)
{
    Vector<M,T> b;
    for(unsigned int i = 0;i < M; i++)
    {
        T sum=T(0);
        for(unsigned int j = 0;j < N; j++)
        {
            sum+=A(i,j) * x[j];
        }
        setAt(b,i,sum);
    }
    return b;
}

/** Matrix Vector multiply with the matrix transposed
    @relates Vector
    */
template<int M, int N, class T>
inline Vector<M,T> transposeMultiply(const Matrix<M,N,T> &A,
        const Vector<N,T> &x)
{
    Vector<M,T> b;
    for(unsigned int i = 0;i < M; i++)
    {
        T sum=T(0);
        for(unsigned int j = 0;j < N; j++)
        {
            sum+=A(j,i) * x[j];
        }
        setAt(b,i,sum);
    }
    return b;
}

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

/** Matrix print
    @relates Matrix
    */
template<int M, int N, class T>
inline String fprint(FILE *a_fp, const Matrix<M,N,T> &a_m)
{
    String s;
    for(unsigned int i = 0;i < M; i++)
    {
        for(unsigned int j = 0;j < N; j++)
        {
            fe_fprintf(a_fp, "%6.3f ", a_m(i,j));
        }
        if(i!=M-1)
        {
            fe_fprintf(a_fp, "\n");
        }
    }
    return s;
}

/** Matrix copy
    @relates Matrix
    */
template<int M, int N, class T,int I, int J, class U>
inline void copy(Matrix<M,N,T> &lhs,const Matrix<I,J,U> &rhs)
{
    int m;
    if(M < I) { m = M; }
    else { m = I; }
    int n;
    if(N < J) { n = N; }
    else { n = J; }

    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);
        }
    }
}

/** Matrix overlay
    @relates Matrix
    */
template<int M, int N, int I, int J, class T>
inline void overlay(Matrix<M,N,T> &lhs,const Matrix<I,J,T> &rhs)
{
    int m;
    if(M < I) { m = M; }
    else { m = I; }
    int n;
    if(N < J) { n = N; }
    else { n = J; }

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


/** Matrix scale in place
    @relates Matrix
    */
template<int M, int N, class T, class U>
inline Matrix<M,N,T> &multiply(Matrix<M,N,T> &A, const U &scale)
{
    for(unsigned int i = 0;i < M*N; i++)
    {
        A[i]*=scale;
    }
    return A;
}

/** Matrix scale
    @relates Matrix
    */
template<int M, int N, class T, class U>
inline Matrix<M,N,T> multiply(const Matrix<M,N,T> &A, const U &scale)
{
    Matrix<M,N,T> tmp;
    for(unsigned int i = 0;i < M*N; i++)
    {
        tmp[i] = scale * A[i];
    }
    return tmp;
}

/** Matrix Matrix add
    @relates Matrix
    */
template<int M, int N, class T, class U>
inline Matrix<M,N,T> operator+(const Matrix<M,N,T> &lhs,
        const Matrix<M,N,U> &rhs)
{
    Matrix<M,N,T> C;
    add(C,lhs,rhs);
    return C;
}

/** Matrix Matrix subtract
    @relates Matrix
    */
template<int M, int N, class T, class U>
inline Matrix<M,N,T> operator-(const Matrix<M,N,T> &lhs,
        const Matrix<M,N,U> &rhs)
{
    Matrix<M,N,T> C;
    subtract(C,lhs,rhs);
    return C;
}

/** Matrix Matrix multiply
    @relates Matrix
    */
template<int M, int N, int L, class T, class U>
inline Matrix<M,L,T> operator*(const Matrix<M,N,T> &lhs,
        const Matrix<N,L,U> &rhs)
{
    Matrix<M,L,T> C;
    multiply(C,lhs,rhs);
    return C;
}

/** Matrix Scale
    @relates Matrix
    */
template<int M, int N, class T>
inline Matrix<M,N,T> operator*(const Matrix<M,N,T> &lhs, const Real rhs)
{
    return multiply(lhs,rhs);
}

/** Matrix Vector multiply
    @relates Vector
    */
template<int M, int N, class T, class U>
inline Vector<M,U> operator*(const Matrix<M,N,T> &lhs, const Vector<N,U> &rhs)
{
    Vector<M,T> b;
    multiply(b,lhs,rhs);
    return b;
}

/** Matrix Scale
    @relates Matrix
    */
template<int M, int N, class T, class U>
inline Matrix<M,N,T> operator*(const U lhs, const Matrix<M,N,T> &rhs)
{
    return multiply(rhs,lhs);
}

/** Matrix Scale
    @relates Matrix
    */
template<int M, int N, class T, class U>
inline Matrix<M,N,T> &operator*=(Matrix<M,N,T> &lhs, const U rhs)
{
    return multiply(lhs,rhs);
}

/** Square root of a matrix.
    This is a Cholesky Factorization: trans(U)*U = A
    */
template<int M, int N, class T>
inline void squareroot(Matrix<M,N,T> &a_U, const Matrix<M,N,T> &a_A)
{
#if 1
    FEASSERT(M==N);
    a_U = a_A;
    for(unsigned int i = 0; i < N-1; i++)
    {
        //fe_fprintf(stderr, "pre [%d %g]\n", i, a_U(i,i));
        if(a_U(i,i) < 0.0)
        {
            //fe_fprintf(stderr, "not positive definite, negative diagonal [%d %g]\n", i, a_U(i,i));
            //a_U(i,i) = 1.0;
            assert(0);
        }
#if FE_COMPILER != FE_MICROSOFT
        if(isnan(a_U(i,i)))
        {
            //fe_fprintf(stderr, "corruption, nan %d\n", i);
            assert(0);
        }
#endif
        a_U(i,i) = sqrt(a_U(i,i));
#if FE_COMPILER != FE_MICROSOFT
        if(isnan(a_U(i,i)))
        {
            //fe_fprintf(stderr, "diagonal post sqrt, nan %d\n", i);
            assert(0);
        }
#endif
        if(a_U(i,i) <= 0.001) {
            //fe_fprintf(stderr, "not positive definite [%g]\n", a_U(i,i));
            assert(0);
        }
        else
        {
            //fe_fprintf(stderr, "-- %d %g\n", i, a_U(i,i));
        }
        T z = 1.0 / a_U(i,i);
#if FE_COMPILER != FE_MICROSOFT
        if(isnan(z))
        {
            //fe_fprintf(stderr, "z, nan %d\n", i);
            assert(0);
        }
#endif
        for(unsigned int j = i+1; j < N; j++)
        {
#if FE_COMPILER != FE_MICROSOFT
            if(isnan(a_U(i,j)))
            {
                //fe_fprintf(stderr, "off diag, nan %d %d\n", i, j);
                assert(0);
            }
#endif
            a_U(i,j) *= z;
        }
        for(unsigned int j = i+1; j < N; j++)
        {
            for(unsigned int jj = j; jj < N; jj++)
            {
                a_U(j,jj) -= a_U(i,j) * a_U(i,jj);
#if FE_COMPILER != FE_MICROSOFT
                if(isnan(a_U(j,jj)))
                {
                    //fe_fprintf(stderr, "off diag 2, nan %d %d\n", j, jj);
                    assert(0);
                }
#endif
            }
        }
    }

#if FE_COMPILER != FE_MICROSOFT
    if(isnan(a_U(N-1,N-1)))
    {
        //fe_fprintf(stderr, "N-1,N-1, presqrt\n");
        assert(0);
    }
#endif
//fe_fprintf(stderr, "=-==- %g\n", a_U(N-1,N-1));
    if(a_U(N-1,N-1) < 0.0)
    {
        //fe_fprintf(stderr, "not positive definite, negative diagonal [%d %g]\n", N-1, a_U(N-1,N-1));
        //a_U(N-1,N-1) = 1.0;
        assert(0);
    }
    a_U(N-1,N-1) = sqrt(a_U(N-1,N-1));
#if FE_COMPILER != FE_MICROSOFT
    if(isnan(a_U(N-1,N-1)))
    {
        //fe_fprintf(stderr, "N-1,N-1, postsqrt\n");
        assert(0);
    }
#endif
#endif



#if 0
    arma::Mat<double> A(a_A.raw(),3,3);

    arma::Mat<double> U(a_U.raw(),3,3,false,true);

    try
    {
        if(!arma::chol(U, A))
        {
            fprint(stderr, a_A);
            exit(101);
        }
    }
    catch(...)
    {
        fprint(stderr, a_A);
        exit(102);
    }
#endif

}

template<int M, int N, class T>
inline void backSub(const Matrix<M,N,T> &a_A, Vector<N,T> &a_x, const Vector<N,T> &a_b)
{
    FEASSERT(M==N);
    const unsigned int n_1 = N-1;
    a_x[n_1] = a_b[n_1]/a_A(n_1, n_1);

    for(int i = N-2; i >= 0; i--)
    {
        a_x[i] = a_b[i];
        for(int j = i+1; j < N; j++)
        {
            a_x[i] -= a_A(i, j)*a_x[j];
        }
        a_x[i] = a_x[i] / a_A(i, i);
    }
}

// operates on upper tri as if it is a transpose of the upper and is lower
template<int M, int N, class T>
inline void transposeForeSub(const Matrix<M,N,T> &a_A, Vector<N,T> &a_x, const Vector<N,T> &a_b)
{
    FEASSERT(M==N);
    a_x[0] = a_b[0]/a_A(0, 0);
    for(unsigned int i = 1; i < N; i++)
    {
        a_x[i] = a_b[i];
        for(unsigned int j = 0; j < i; j++)
        {
            a_x[i] -= a_A(j, i)*a_x[j];
        }
        a_x[i] = a_x[i] / a_A(i, i);
    }
}

/** 4x4 invert
    */
template <typename T>
inline Matrix<4,4,T> &osg_invert(Matrix<4,4,T> &a_inverted,
        const Matrix<4,4,T> &a_matrix)
{
    /* Copied from OpenSceneGraph.org (transposed for column major) */
    T   r00, r01, r02, r10, r11, r12, r20, r21, r22;

    // Copy rotation components directly into variables for speed
    r00 = a_matrix(0,0); r01 = a_matrix(0,1); r02 = a_matrix(0,2);
    r10 = a_matrix(1,0); r11 = a_matrix(1,1); r12 = a_matrix(1,2);
    r20 = a_matrix(2,0); r21 = a_matrix(2,1); r22 = a_matrix(2,2);

    // Partially compute inverse of rot
    a_inverted(0,0) = r11*r22 - r21*r12;
    a_inverted(1,0) = r20*r12 - r10*r22;
    a_inverted(2,0) = r10*r21 - r20*r11;

    // Compute determinant of rot from 3 elements just computed
    T one_over_det =
        1.0/(r00*a_inverted(0,0) + r01*a_inverted(1,0) + r02*a_inverted(2,0));
    r00 *= one_over_det; r01 *= one_over_det; r02 *= one_over_det;

    // Finish computing inverse of rot
    a_inverted(0,0) *= one_over_det;
    a_inverted(1,0) *= one_over_det;
    a_inverted(2,0) *= one_over_det;
    a_inverted(3,0) = 0.0;
    a_inverted(0,1) = r21*r02 - r01*r22; // Have already been divided by det
    a_inverted(1,1) = r00*r22 - r20*r02; // same
    a_inverted(2,1) = r20*r01 - r00*r21; // same
    a_inverted(3,1) = 0.0;
    a_inverted(0,2) = r01*r12 - r11*r02; // Have already been divided by det
    a_inverted(1,2) = r10*r02 - r00*r12; // same
    a_inverted(2,2) = r00*r11 - r10*r01; // same
    a_inverted(3,2) = 0.0;
    a_inverted(3,3) = 1.0;

    // We no longer need the rxx or det variables anymore, so we can reuse
    // them for whatever we want.  But we will still rename them for the
    // sake of clarity.

#define d r22
    d = a_matrix(3,3);

    T s = d-1.0;
    if(s*s > 1.0e-6 ) // Involves perspective, so we compute the full inverse
    {
        Matrix<4,4,T> TPinv;
        a_inverted(0,3) = 0.0;
        a_inverted(1,3) = 0.0;
        a_inverted(2,3) = 0.0;

#define px r00
#define py r10
#define pz r20
#define one_over_s  one_over_det
#define a  r01
#define b  r11
#define c  r21

        a = a_matrix(3,0);
        b = a_matrix(3,1);
        c = a_matrix(3,2);
        px = a_inverted(0,0)*a + a_inverted(1,0)*b + a_inverted(2,0)*c;
        py = a_inverted(0,1)*a + a_inverted(1,1)*b + a_inverted(2,1)*c;
        pz = a_inverted(0,2)*a + a_inverted(1,2)*b + a_inverted(2,2)*c;

#undef a
#undef b
#undef c
#define tx r01
#define ty r11
#define tz r21

        tx = a_matrix(0,3);
        ty = a_matrix(1,3);
        tz = a_matrix(2,3);
        one_over_s = 1.0/(d - (tx*px + ty*py + tz*pz));

        tx *= one_over_s; ty *= one_over_s; tz *= one_over_s;

        // Compute inverse of trans*corr
        TPinv(0,0) = tx*px + 1.0;
        TPinv(1,0) = ty*px;
        TPinv(2,0) = tz*px;
        TPinv(3,0) = -px * one_over_s;
        TPinv(0,1) = tx*py;
        TPinv(1,1) = ty*py + 1.0;
        TPinv(2,1) = tz*py;
        TPinv(3,1) = -py * one_over_s;
        TPinv(0,2) = tx*pz;
        TPinv(1,2) = ty*pz;
        TPinv(2,2) = tz*pz + 1.0;
        TPinv(3,2) = -pz * one_over_s;
        TPinv(0,3) = -tx;
        TPinv(1,3) = -ty;
        TPinv(2,3) = -tz;
        TPinv(3,3) = one_over_s;

        Matrix<4,4,T> preMult(a_inverted);
        multiply(a_inverted, preMult, TPinv);

#undef px
#undef py
#undef pz
#undef one_over_s
#undef d
    }
    else // bottommost row is [0; 0; 0; 1] so it can be ignored
    {
        tx = a_matrix(0,3);
        ty = a_matrix(1,3);
        tz = a_matrix(2,3);

        // Compute translation components of mat'
        a_inverted(0,3) =
            -(tx*a_inverted(0,0) + ty*a_inverted(0,1) + tz*a_inverted(0,2));
        a_inverted(1,3) =
            -(tx*a_inverted(1,0) + ty*a_inverted(1,1) + tz*a_inverted(1,2));
        a_inverted(2,3) =
            -(tx*a_inverted(2,0) + ty*a_inverted(2,1) + tz*a_inverted(2,2));

#undef tx
#undef ty
#undef tz
    }

    return a_inverted;
}

template <typename T>
inline T determinant3x3(    T a1,T a2,T a3,
                    T b1,T b2,T b3,
                    T c1,T c2,T c3)
{
    return  a1*(b2*c3-b3*c2)
            -b1*(a2*c3-a3*c2)
            +c1*(a2*b3-a3*b2);
}

template <typename T>
inline T determinant(const Matrix<4,4,T> &m)
{
    return
    m[0]*determinant3x3(m[5],m[6],m[7],m[9],m[10],m[11],m[13],m[14],m[15])
    -m[4]*determinant3x3(m[1],m[2],m[3],m[9],m[10],m[11],m[13],m[14],m[15])
    +m[8]*determinant3x3(m[1],m[2],m[3],m[5],m[6],m[7],m[13],m[14],m[15])
    -m[12]*determinant3x3(m[1],m[2],m[3],m[5],m[6],m[7],m[9],m[10],m[11]);
}

template <typename T>
inline Matrix<4,4,T> &jad_invert(Matrix<4,4,T> &a_inverted,
        const Matrix<4,4,T> &a_matrix)
{
    Matrix<4,4,T> m(a_matrix);
    setTranspose(m);
    const F32 det=determinant(m);

    //* If determinant not valid, can't compute inverse (return identity)
    if(fabsf(det)<1e-8)
    {
        feX("inversion failure");
        return setIdentity(a_inverted);
    }

    const F32 inv=1.0f/det;

    a_inverted[0]=   inv*determinant3x3(
                        m[5],m[6],m[7],m[9],m[10],m[11],m[13],m[14],m[15]);
    a_inverted[4]=  -inv*determinant3x3(
                        m[1],m[2],m[3],m[9],m[10],m[11],m[13],m[14],m[15]);
    a_inverted[8]=   inv*determinant3x3(
                        m[1],m[2],m[3],m[5],m[6],m[7],m[13],m[14],m[15]);
    a_inverted[12]= -inv*determinant3x3(
                        m[1],m[2],m[3],m[5],m[6],m[7],m[9],m[10],m[11]);

    a_inverted[1]=  -inv*determinant3x3(
                        m[4],m[6],m[7],m[8],m[10],m[11],m[12],m[14],m[15]);
    a_inverted[5]=   inv*determinant3x3(
                        m[0],m[2],m[3],m[8],m[10],m[11],m[12],m[14],m[15]);
    a_inverted[9]=  -inv*determinant3x3(
                        m[0],m[2],m[3],m[4],m[6],m[7],m[12],m[14],m[15]);
    a_inverted[13]=  inv*determinant3x3(
                        m[0],m[2],m[3],m[4],m[6],m[7],m[8],m[10],m[11]);

    a_inverted[2]=   inv*determinant3x3(
                        m[4],m[5],m[7],m[8],m[9],m[11],m[12],m[13],m[15]);
    a_inverted[6]=  -inv*determinant3x3(
                        m[0],m[1],m[3],m[8],m[9],m[11],m[12],m[13],m[15]);
    a_inverted[10]=  inv*determinant3x3(
                        m[0],m[1],m[3],m[4],m[5],m[7],m[12],m[13],m[15]);
    a_inverted[14]= -inv*determinant3x3(
                        m[0],m[1],m[3],m[4],m[5],m[7],m[8],m[9],m[11]);

    a_inverted[3]=  -inv*determinant3x3(
                        m[4],m[5],m[6],m[8],m[9],m[10],m[12],m[13],m[14]);
    a_inverted[7]=   inv*determinant3x3(
                        m[0],m[1],m[2],m[8],m[9],m[10],m[12],m[13],m[14]);
    a_inverted[11]= -inv*determinant3x3(
                        m[0],m[1],m[2],m[4],m[5],m[6],m[12],m[13],m[14]);
    a_inverted[15]=  inv*determinant3x3(
                        m[0],m[1],m[2],m[4],m[5],m[6],m[8],m[9],m[10]);

    return a_inverted;

}

template <typename T>
inline Matrix<4,4,T> &brk_invert(Matrix<4,4,T> &a_inverted,
        const Matrix<4,4,T> &a_matrix)
{
    F32 src[16];
    F32 tmp[12];
    F32 det;
    F32 dst[16];

    for(IWORD a = 0; a < 16; a++)
    {
        src[a] = a_matrix[a];
    }

    // calculate pairs for first 8 cofactors
    tmp[0] = src[10] * src[15];
    tmp[1] = src[11] * src[14];
    tmp[2] = src[9] * src[15];
    tmp[3] = src[11] * src[13];
    tmp[4] = src[9] * src[14];
    tmp[5] = src[10] * src[13];
    tmp[6] = src[8] * src[15];
    tmp[7] = src[11] * src[12];
    tmp[8] = src[8] * src[14];
    tmp[9] = src[10] * src[12];
    tmp[10] = src[8] * src[13];
    tmp[11] = src[9] * src[12];

    // calculate first 8 cofactors
    dst[0] = tmp[0]* src[5] + tmp[3]*src[6] + tmp[4]*src[7];
    dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
    dst[1] = tmp[1]* src[4] + tmp[6]*src[6] + tmp[9]*src[7];
    dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
    dst[2] = tmp[2]* src[4] + tmp[7]*src[5] + tmp[10]*src[7];
    dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
    dst[3] = tmp[5]* src[4] + tmp[8]*src[5] + tmp[11]*src[6];
    dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
    dst[4] = tmp[1]* src[1] + tmp[2]*src[2] + tmp[5]*src[3];
    dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
    dst[5] = tmp[0]* src[0] + tmp[7]*src[2] + tmp[8]*src[3];
    dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
    dst[6] = tmp[3]* src[0] + tmp[6]*src[1] + tmp[11]*src[3];
    dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
    dst[7] = tmp[4]* src[0] + tmp[9]*src[1] + tmp[10]*src[2];
    dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];

    // calculate pairs for first 8 cofactors
    tmp[0] = src[2] * src[7];
    tmp[1] = src[3] * src[6];
    tmp[2] = src[1] * src[7];
    tmp[3] = src[3] * src[5];
    tmp[4] = src[1] * src[6];
    tmp[5] = src[2] * src[5];
    tmp[6] = src[0] * src[7];
    tmp[7] = src[3] * src[4];
    tmp[8] = src[0] * src[6];
    tmp[9] = src[2] * src[4];
    tmp[10] = src[0] * src[5];
    tmp[11] = src[1] * src[4];

    // calculate second 8 cofactors
    dst[8] = tmp[0]* src[13] + tmp[3]*src[14] + tmp[4]*src[15];
    dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
    dst[9] = tmp[1]* src[12] + tmp[6]*src[14] + tmp[9]*src[15];
    dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
    dst[10] = tmp[2]* src[12] + tmp[7]*src[13] + tmp[10]*src[15];
    dst[10] -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
    dst[11] = tmp[5]* src[12] + tmp[8]*src[13] + tmp[11]*src[14];
    dst[11] -= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
    dst[12] = tmp[2]* src[10] + tmp[5]*src[11] + tmp[1]*src[9];
    dst[12] -= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
    dst[13] = tmp[8]* src[11] + tmp[0]*src[8] + tmp[7]*src[10];
    dst[13] -= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
    dst[14] = tmp[6]* src[9] + tmp[11]*src[11] + tmp[3]*src[8];
    dst[14] -= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
    dst[15] = tmp[10]* src[10] + tmp[4]*src[8] + tmp[9]*src[9];
    dst[15] -= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];

    // calculate determinate
    det = determinant(a_matrix);

    // calculate matrix inverse
    det = 1.0/det;
    for(IWORD j = 0; j < 16; j++)
    {
        dst[j] *= det;
    }

    for(IWORD i = 0; i < 4; i++)
    {
        a_inverted[i] = dst[i + 4];
        a_inverted[i + 4] = dst[i * 4 + 1];
        a_inverted[i + 8] = dst[i * 4 + 2];
        a_inverted[i + 12] = dst[i * 4 + 3];
    }

    return a_inverted;

}

/// 4x4 full matrix inversion
template <typename T>
inline Matrix<4,4,T> &invert(Matrix<4,4,T> &a_inverted,
        const Matrix<4,4,T> &a_matrix)
{
    return osg_invert(a_inverted, a_matrix);
}


// TODO: test/validate this
/// 3x3 matrix inversion
template <typename T>
inline Matrix<3,3,T> &invert(Matrix<3,3,T> &a_inverted,
        const Matrix<3,3,T> &a_matrix)
{
    a_inverted(0,0)=  a_matrix(1,1)*a_matrix(2,2) - a_matrix(2,1)*a_matrix(1,2);
    a_inverted(0,1)= -a_matrix(0,1)*a_matrix(2,2) + a_matrix(2,1)*a_matrix(0,2);
    a_inverted(0,2)=  a_matrix(0,1)*a_matrix(1,2) - a_matrix(1,1)*a_matrix(0,2);
    a_inverted(1,0)= -a_matrix(1,0)*a_matrix(2,2) + a_matrix(2,0)*a_matrix(1,2);
    a_inverted(1,1)=  a_matrix(0,0)*a_matrix(2,2) - a_matrix(2,0)*a_matrix(0,2);
    a_inverted(1,2)= -a_matrix(0,0)*a_matrix(1,2) + a_matrix(1,0)*a_matrix(0,2);
    a_inverted(2,0)=  a_matrix(1,0)*a_matrix(2,1) - a_matrix(2,0)*a_matrix(1,1);
    a_inverted(2,1)= -a_matrix(0,0)*a_matrix(2,1) + a_matrix(2,0)*a_matrix(0,1);
    a_inverted(2,2)=  a_matrix(0,0)*a_matrix(1,1) - a_matrix(1,0)*a_matrix(0,1);

    Real det = a_inverted(0,0) * a_matrix(0,0) + a_inverted(1,0) * a_matrix(0,1)
        + a_inverted(2,0) * a_matrix(0,2);

    if(fabs(det) < fe::tol)
    {
        feX("attempt to invert singular 3x3 matrix");
    }

    a_inverted *= 1.0 / det;

    return a_inverted;
}

template <typename T>
inline bool inverted(Matrix<3,3,T> &a_inverted,
        const Matrix<3,3,T> &a_matrix)
{
    a_inverted(0,0)=  a_matrix(1,1)*a_matrix(2,2) - a_matrix(2,1)*a_matrix(1,2);
    a_inverted(0,1)= -a_matrix(0,1)*a_matrix(2,2) + a_matrix(2,1)*a_matrix(0,2);
    a_inverted(0,2)=  a_matrix(0,1)*a_matrix(1,2) - a_matrix(1,1)*a_matrix(0,2);
    a_inverted(1,0)= -a_matrix(1,0)*a_matrix(2,2) + a_matrix(2,0)*a_matrix(1,2);
    a_inverted(1,1)=  a_matrix(0,0)*a_matrix(2,2) - a_matrix(2,0)*a_matrix(0,2);
    a_inverted(1,2)= -a_matrix(0,0)*a_matrix(1,2) + a_matrix(1,0)*a_matrix(0,2);
    a_inverted(2,0)=  a_matrix(1,0)*a_matrix(2,1) - a_matrix(2,0)*a_matrix(1,1);
    a_inverted(2,1)= -a_matrix(0,0)*a_matrix(2,1) + a_matrix(2,0)*a_matrix(0,1);
    a_inverted(2,2)=  a_matrix(0,0)*a_matrix(1,1) - a_matrix(1,0)*a_matrix(0,1);

    Real det = a_inverted(0,0) * a_matrix(0,0) + a_inverted(1,0) * a_matrix(0,1)
        + a_inverted(2,0) * a_matrix(0,2);

    if(fabs(det) < fe::tol)
    {
        return false;
    }

    a_inverted *= 1.0 / det;

    return true;
}

/// @brief Create perspective projection transform just like gluPerspective
template <typename T>
inline Matrix<4,4,T> perspective(T fovy,T aspect,T nearplane,T farplane)
{
    FEASSERT(fovy>0.0f);
    FEASSERT(aspect>0.0f);
    FEASSERT(nearplane>0.0f);
    FEASSERT(farplane>nearplane);

    Matrix<4,4,T> matrix;
    set(matrix);

    T f=1.0f/tan(0.5f*fovy*degToRad);
    T d=nearplane-farplane;

    matrix[0]=f/aspect;
    matrix[5]=f;
    matrix[10]=(nearplane+farplane)/d;
    matrix[11]= -1.0f;
    matrix[14]=2.0f*nearplane*farplane/d;

    return matrix;
}

/// @brief Try to extract settings of a perspective matrix
template <typename T>
inline void decomposePerspective(const Matrix<4,4,T>& a_rProjection,
    T &a_rFovy,T &a_rAspect,T &a_rNearplane,T &a_rFarplane)
{
        const Real a=a_rProjection[10];
        const Real b=a_rProjection[14];

        a_rAspect=a_rProjection[5]/a_rProjection[0];
        a_rFovy=2.0/degToRad*atan(1.0/a_rProjection[5]);
        a_rNearplane=0.5*(b*(a+1.0)/(a-1.0)-b);
        a_rFarplane=a_rNearplane*(a-1.0)/(a+1.0);

/*
        a=m[10]
        b=m[14]

        a=(n+f)/(n-f)
        b=2nf/(n-f)

        an-af=n+f
        bn-bf=2nf

        an-n=af+f
        n(a-1)=f(a+1)
        f=n(a-1)/(a+1)

        bn=f(2n+b)
        f=bn/(2n+b)
        n(a-1)/(a+1)=bn/(2n+b)
        (a-1)/(a+1)=b/(2n+b)
        (a-1)(2n+b)=b(a+1)
        2n+b=b(a+1)/(a-1)
        n=(b(a+1)/(a-1)-b)/2
*/
}

/// @brief Create orthogonal projection transform just like glOrtho
template <typename T>
inline Matrix<4,4,T> ortho(T left,T right,T bottom, T top,T near_val,T far_val)
{
    Matrix<4,4,T> matrix;
    set(matrix);

    T dx = right-left;
    T dy = top-bottom;
    T dz = far_val-near_val;

    matrix[0] = 2.0/dx;
    matrix[5] = 2.0/dy;
    matrix[10] = -2.0/dz;

    matrix[12] = -(right+left)/dx;
    matrix[13] = -(top+bottom)/dy;
    matrix[14] = -(far_val+near_val)/dz;
    matrix[15] = 1.0;

    return matrix;
}

/// project vector through matrix.  divides by transformed w
template <typename T>
inline void project(Vector<4,T> &a_r, const Matrix<4,4,T> &a_m,
        const Vector<4,T> &a_v)
{
    const T w=1.0f/(a_v[0]*a_m[3]+a_v[1]*a_m[7]+a_v[2]*a_m[11]+a_m[15] );
    a_r[0]=a_v[0]*a_m[0]+a_v[1]*a_m[4]+a_v[2]*a_m[8]+a_m[12];
    a_r[1]=a_v[0]*a_m[1]+a_v[1]*a_m[5]+a_v[2]*a_m[9]+a_m[13];
    a_r[2]=a_v[0]*a_m[2]+a_v[1]*a_m[6]+a_v[2]*a_m[10]+a_m[14];

    a_r[0]*=w;
    a_r[1]*=w;
    a_r[2]*=w;
}

/// reverse project vector through matrix.  reverses w division
template <typename T>
inline void unproject(Vector<4,T> &a_r, const Matrix<4,4,T> &a_m,
        const Vector<4,T> &a_v)
{
    const Vector<4,T> inverted=inverseTransformVector(a_m,a_v);
    if(inverted[3]==T(0))
    {
        set(a_r,0,0,0,0);
        return;
    }

    a_r[3]=T(1)/inverted[3];
    a_r[0]=inverted[0]*a_r[3];
    a_r[1]=inverted[1]*a_r[3];
    a_r[2]=inverted[2]*a_r[3];
}

template <typename T>
inline void transformVector(const Matrix<4,4,T> &a_m, const Vector<4,T> &a_v,
        Vector<4,T> &a_r)
{
    set(a_r,
            a_v[0]*a_m[0]   +a_v[1]*a_m[4]  +a_v[2]*a_m[8]  +a_v[3]*a_m[12],
            a_v[0]*a_m[1]   +a_v[1]*a_m[5]  +a_v[2]*a_m[9]  +a_v[3]*a_m[13],
            a_v[0]*a_m[2]   +a_v[1]*a_m[6]  +a_v[2]*a_m[10] +a_v[3]*a_m[14],
            a_v[0]*a_m[3]   +a_v[1]*a_m[7]  +a_v[2]*a_m[11] +a_v[3]*a_m[15]);
}

template <int N, typename T>
inline Vector<N,T> transformVector(const Matrix<4,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<4,4,T>& lhs,
        const Vector<N,T>& in)
{
    FEASSERT(N>2);
    Matrix<4,4,T> inverse;
    invert(inverse,lhs);
    return transformVector(inverse,in);
}

template <typename T>
inline void transposeTransformVector(const Matrix<4,4,T> &a_m,
        const Vector<4,T> &a_v,
        Vector<4,T> &a_r)
{
    set(a_r,
            a_v[0]*a_m[0]   +a_v[1]*a_m[1]  +a_v[2]*a_m[2]  +a_v[3]*a_m[3],
            a_v[0]*a_m[4]   +a_v[1]*a_m[5]  +a_v[2]*a_m[6]  +a_v[3]*a_m[7],
            a_v[0]*a_m[8]   +a_v[1]*a_m[9]  +a_v[2]*a_m[10] +a_v[3]*a_m[11],
            a_v[0]*a_m[12]  +a_v[1]*a_m[13] +a_v[2]*a_m[14] +a_v[3]*a_m[15]);
}

/** @brief Compute the per-element product of the vector and the
    inverse diagonal entries

    @relates Matrix

    Only the diagonal elements are used.  The non-diagonal values are
    not read and treated as zero.
*/
template<int N,typename T>
inline void premultiplyInverseDiagonal(
        Vector<N,T>& result,
        const Matrix<N,N,T>& diagonal,
        const Vector<N,T>& vector)
{
    //* result[i] = vector[i] / diag[i][i]

    for(U32 m=0;m<N;m++)
    {
        result[m]=vector[m]/diagonal(m,m);
    }
}

/** Matrix-Matrix multiply where first matrix is presumed diagonal
    @relates Matrix

    This amounts to a scale of each row by the reciprical of the
    corresponding diagonal element.

    Diagonal elements of the first matrix are inverted during the multiply.
    Non-diagonal elements of the first matrix are ignored and treated as zero.
    */
template<int N,class T>
inline Matrix<N,N,T>& premultiplyInverseDiagonal(
        Matrix<N,N,T> &result,
        const Matrix<N,N,T> &lhs,const Matrix<N,N,T> &rhs)
{
    for(U32 i=0;i<N;i++)
    {
        T d=T(1)/lhs(i,i);
        for(U32 j=0;j<N;j++)
        {
            result(i,j)=rhs(i,j)*d;
        }
    }
    return result;
}

/** Matrix-Matrix multiply where first matrix is presumed diagonal
    @relates Matrix

    This amounts to a scale of each column by the reciprical of the
    corresponding diagonal element.

    Diagonal elements of the second matrix are inverted during the multiply.
    Non-diagonal elements of the second matrix are ignored and treated as zero.
    */
template<int N,class T>
inline Matrix<N,N,T>& postmultiplyInverseDiagonal(
        Matrix<N,N,T> &result,
        const Matrix<N,N,T> &lhs,const Matrix<N,N,T> &rhs)
{
    for(U32 j=0;j<N;j++)
    {
        T d=T(1)/rhs(j,j);
        for(U32 i=0;i<N;i++)
        {
            result(i,j)=lhs(i,j)*d;
        }
    }
    return result;
}

template<int N,class T>
inline Matrix<N,N,T>& outerProduct(
    Matrix<N,N,T> &result, const Vector<N,T> &a, const Vector<N,T> &b)
{
    for(unsigned int i = 0; i < N; i++)
    {
        for(unsigned int j = 0; j < N; j++)
        {
            result(i,j) = a[i] * b[j];
        }
    }

    return result;
}

/** 3D Matrix rotation
    @relates Matrix

    Rotate a 3x3 matrix counterclockwise about the specified axis
    */template <typename T,typename U>
inline Matrix<3,3,T>& rotateMatrix(Matrix<3,3,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;

    return lhs;
}

} /* namespace */

#endif /* __math_matrix_h__ */