matrix.h

Go to the documentation of this file.

/***************************************************************************************************
 * Copyright 2023 NVIDIA Corporation. All rights reserved.
 **************************************************************************************************/
 
#ifndef MI_MATH_MATRIX_H
#define MI_MATH_MATRIX_H
 
#include <mi/base/types.h>
#include <mi/math/assert.h>
#include <mi/math/function.h>
#include <mi/math/vector.h>
 
namespace mi {
 
namespace math {
 
//------- POD struct that provides storage for matrix elements --------
 
template <typename T, Size ROW, Size COL>
struct Matrix_struct
{
    T elements[ROW*COL]; 
};
 
template <typename T> struct Matrix_struct<T,1,1>
{
    T xx;  
};
 
template <typename T> struct Matrix_struct<T,2,1>
{
    T xx;  
    T yx;  
};
 
template <typename T> struct Matrix_struct<T,3,1>
{
    T xx;  
    T yx;  
    T zx;  
};
 
template <typename T> struct Matrix_struct<T,4,1>
{
    T xx;  
    T yx;  
    T zx;  
    T wx;  
};
 
template <typename T> struct Matrix_struct<T,1,2>
{
    T xx;  
    T xy;  
};
 
template <typename T> struct Matrix_struct<T,2,2>
{
    T xx;  
    T xy;  
    T yx;  
    T yy;  
};
 
template <typename T> struct Matrix_struct<T,3,2>
{
    T xx;  
    T xy;  
    T yx;  
    T yy;  
    T zx;  
    T zy;  
};
 
template <typename T> struct Matrix_struct<T,4,2>
{
    T xx;  
    T xy;  
    T yx;  
    T yy;  
    T zx;  
    T zy;  
    T wx;  
    T wy;  
};
 
template <typename T> struct Matrix_struct<T,1,3>
{
    T xx;  
    T xy;  
    T xz;  
};
 
template <typename T> struct Matrix_struct<T,2,3>
{
    T xx;  
    T xy;  
    T xz;  
    T yx;  
    T yy;  
    T yz;  
};
 
template <typename T> struct Matrix_struct<T,3,3>
{
    T xx;  
    T xy;  
    T xz;  
    T yx;  
    T yy;  
    T yz;  
    T zx;  
    T zy;  
    T zz;  
};
 
template <typename T> struct Matrix_struct<T,4,3>
{
    T xx;  
    T xy;  
    T xz;  
    T yx;  
    T yy;  
    T yz;  
    T zx;  
    T zy;  
    T zz;  
    T wx;  
    T wy;  
    T wz;  
};
 
template <typename T> struct Matrix_struct<T,1,4>
{
    T xx;  
    T xy;  
    T xz;  
    T xw;  
};
 
template <typename T> struct Matrix_struct<T,2,4>
{
    T xx;  
    T xy;  
    T xz;  
    T xw;  
    T yx;  
    T yy;  
    T yz;  
    T yw;  
};
 
template <typename T> struct Matrix_struct<T,3,4>
{
    T xx;  
    T xy;  
    T xz;  
    T xw;  
    T yx;  
    T yy;  
    T yz;  
    T yw;  
    T zx;  
    T zy;  
    T zz;  
    T zw;  
};
 
template <typename T> struct Matrix_struct<T,4,4>
{
    T xx;  
    T xy;  
    T xz;  
    T xw;  
    T yx;  
    T yy;  
    T yz;  
    T yw;  
    T zx;  
    T zy;  
    T zz;  
    T zw;  
    T wx;  
    T wy;  
    T wz;  
    T ww;  
};
 
//------ Indirect access to matrix storage base ptr to keep Matrix_struct a POD --
 
// Helper class to determine the matrix struct base pointer of the generic
// #Matrix_struct (\c specialized==\c false).
template <typename T, class Matrix, bool specialized>
struct Matrix_struct_get_base_pointer
{
    static inline T*       get_base_ptr( Matrix& m)       { return m.elements; }
    static inline const T* get_base_ptr( const Matrix& m) { return m.elements; }
};
 
// Specialization of helper class to determine the matrix struct base pointer
// of a specialized #Matrix_struct (\c specialized==\c true).
template <typename T, class Matrix>
struct Matrix_struct_get_base_pointer<T,Matrix,true>
{
    static inline T*       get_base_ptr( Matrix& m)       { return &m.xx; }
    static inline const T* get_base_ptr( const Matrix& m) { return &m.xx; }
};
 
 
template <typename T, Size ROW, Size COL>
inline T* matrix_base_ptr( Matrix_struct<T,ROW,COL>& mat)
{
    return Matrix_struct_get_base_pointer<T,Matrix_struct<T,ROW,COL>,
               (ROW<=4 && COL<=4)>::get_base_ptr( mat);
}
 
template <typename T, Size ROW, Size COL>
inline const T* matrix_base_ptr( const Matrix_struct<T,ROW,COL>& mat)
{
    return Matrix_struct_get_base_pointer<T,Matrix_struct<T,ROW,COL>,
               (ROW<=4 && COL<=4)>::get_base_ptr( mat);
}
 // end group mi_math_matrix_struct
 
 
template <typename T, Size ROW, Size COL>
class Matrix : public Matrix_struct<T,ROW,COL> //-V690 PVS
{
public:
    typedef Matrix_struct<T,ROW,COL> Pod_type;     
    typedef Matrix_struct<T,ROW,COL> storage_type; 
 
    typedef T           value_type;                
    typedef Size        size_type;                 
    typedef Difference  difference_type;           
    typedef T *         pointer;                   
    typedef const T *   const_pointer;             
    typedef T &         reference;                 
    typedef const T &   const_reference;           
 
    typedef Vector<T,COL>  Row_vector;
 
    typedef Vector<T,ROW>  Column_vector;
 
    static const Size ROWS    = ROW;      
    static const Size COLUMNS = COL;      
    static const Size SIZE    = ROW*COL;  
 
    static inline Size size()     { return SIZE; }
 
    static inline Size max_size() { return SIZE; }
 
    enum Transposed_copy_tag {
        TRANSPOSED_COPY_TAG
    };
 
    inline T * begin() { return mi::math::matrix_base_ptr( *this); }
 
    inline T const * begin() const { return mi::math::matrix_base_ptr( *this); }
 
    inline T * end() {  return begin() + SIZE; }
 
    inline T const * end() const { return begin() + SIZE; }
 
    inline Row_vector & operator[](Size row)
    {
        mi_math_assert_msg( row < ROW, "precondition");
        return reinterpret_cast<Row_vector&>(begin()[row * COL]);
    }
 
    inline const Row_vector & operator[](Size row) const
    {
        mi_math_assert_msg( row < ROW, "precondition");
        return reinterpret_cast<const Row_vector&>(begin()[row * COL]);
    }
 
    inline Matrix() { }
 
#if (__cplusplus >= 201103L)
    Matrix( const Matrix<T,ROW,COL>& other ) = default;
#endif
 
    inline Matrix( const Matrix_struct<T,ROW,COL>& other)
    {
        for( Size i(0u); i < SIZE; ++i)
            begin()[i] = mi::math::matrix_base_ptr( other)[i];
    }
 
    explicit inline Matrix( T diag) 
    {
        for( Size i(0u); i < SIZE; ++i)
            begin()[i] = T(0);
        const Size MIN_DIM = (ROW < COL) ? ROW : COL;
        for( Size k(0u); k < MIN_DIM; ++k)
            begin()[k * COL + k] = diag;
    }
 
    template <typename Iterator>
    inline Matrix( From_iterator_tag, Iterator p)
    {
        for( Size i(0u); i < SIZE; ++i, ++p)
            begin()[i] = *p;
    }
 
    template <typename T2>
    inline explicit Matrix( T2 const (& array)[SIZE])
    {
        for( Size i(0u); i < SIZE; ++i)
            begin()[i] = array[i];
    }
 
    template <typename T2>
    inline explicit Matrix( const Matrix<T2,ROW,COL>& other)
    {
        for( Size i(0u); i < SIZE; ++i)
            begin()[i] = T(other.begin()[i]);
    }
 
    template <typename T2>
    inline explicit Matrix( const Matrix_struct<T2,ROW,COL>& other)
    {
        for( Size i(0u); i < SIZE; ++i)
            begin()[i] = T(mi::math::matrix_base_ptr( other)[i]);
    }
 
    inline Matrix(
        Transposed_copy_tag,
        const Matrix<T,COL,ROW>& other)
    {
        for( Size i(0u); i < ROW; ++i)
            for( Size j(0u); j < COL; ++j)
                begin()[i * COL + j] = other.begin()[j * ROW + i];
    }
 
    template <typename T2>
    inline Matrix(
        Transposed_copy_tag,
        const Matrix<T2,COL,ROW>& other)
    {
        for( Size i(0u); i < ROW; ++i)
            for( Size j(0u); j < COL; ++j)
                begin()[i * COL + j] = T(other.begin()[j * ROW + i]);
    }
 
    inline explicit Matrix(
        const Row_vector& v0)
    {
        mi_static_assert( ROW == 1);
        (*this)[0] = v0;
    }
 
    inline Matrix(
        const Row_vector& v0,
        const Row_vector& v1)
    {
        mi_static_assert( ROW == 2);
        (*this)[0] = v0;
        (*this)[1] = v1;
    }
 
    inline Matrix(
        const Row_vector& v0,
        const Row_vector& v1,
        const Row_vector& v2)
    {
        mi_static_assert( ROW == 3);
        (*this)[0] = v0;
        (*this)[1] = v1;
        (*this)[2] = v2;
    }
 
    inline Matrix(
        const Row_vector& v0,
        const Row_vector& v1,
        const Row_vector& v2,
        const Row_vector& v3)
    {
        mi_static_assert( ROW == 4);
        (*this)[0] = v0;
        (*this)[1] = v1;
        (*this)[2] = v2;
        (*this)[3] = v3;
    }
 
    inline Matrix( T m0, T m1)
    {
        mi_static_assert( SIZE == 2);
        begin()[0]  = m0;  begin()[1]  = m1;
    }
 
    inline Matrix( T m0, T m1, T m2)
    {
        mi_static_assert( SIZE == 3);
        begin()[0]  = m0;  begin()[1]  = m1;  begin()[2]  = m2;
    }
 
    inline Matrix( T m0, T m1, T m2, T m3)
    {
        mi_static_assert( SIZE == 4);
        begin()[0]  = m0;  begin()[1]  = m1;  begin()[2]  = m2;  begin()[3]  = m3;
    }
 
    inline Matrix( T m0, T m1, T m2, T m3, T m4, T m5)
    {
        mi_static_assert( SIZE == 6);
        begin()[0]  = m0;  begin()[1]  = m1;  begin()[2]  = m2;  begin()[3]  = m3;
        begin()[4]  = m4;  begin()[5]  = m5;
    }
 
    inline Matrix( T m0, T m1, T m2, T m3, T m4, T m5, T m6, T m7)
    {
        mi_static_assert( SIZE == 8);
        begin()[0]  = m0;  begin()[1]  = m1;  begin()[2]  = m2;  begin()[3]  = m3;
        begin()[4]  = m4;  begin()[5]  = m5;  begin()[6]  = m6;  begin()[7]  = m7;
    }
 
    inline Matrix( T m0, T m1, T m2, T m3, T m4, T m5, T m6, T m7, T m8)
    {
        mi_static_assert( SIZE == 9);
        begin()[0]  = m0;  begin()[1]  = m1;  begin()[2]  = m2;  begin()[3]  = m3;
        begin()[4]  = m4;  begin()[5]  = m5;  begin()[6]  = m6;  begin()[7]  = m7;
        begin()[8]  = m8;
    }
 
    inline Matrix(
        T  m0, T  m1, T  m2, T  m3,
        T  m4, T  m5, T  m6, T  m7,
        T  m8, T  m9, T m10, T m11)
    {
        mi_static_assert( SIZE == 12);
        begin()[0]  = m0;  begin()[1]  = m1;  begin()[2]  = m2;  begin()[3]  = m3;
        begin()[4]  = m4;  begin()[5]  = m5;  begin()[6]  = m6;  begin()[7]  = m7;
        begin()[8]  = m8;  begin()[9]  = m9;  begin()[10] = m10; begin()[11] = m11;
    }
 
    inline Matrix(
        T  m0, T  m1, T  m2, T  m3,
        T  m4, T  m5, T  m6, T  m7,
        T  m8, T  m9, T m10, T m11,
        T m12, T m13, T m14, T m15)
    {
        mi_static_assert( SIZE == 16);
        begin()[0]  = m0;  begin()[1]  = m1;  begin()[2]  = m2;  begin()[3]  = m3;
        begin()[4]  = m4;  begin()[5]  = m5;  begin()[6]  = m6;  begin()[7]  = m7;
        begin()[8]  = m8;  begin()[9]  = m9;  begin()[10] = m10; begin()[11] = m11;
        begin()[12] = m12; begin()[13] = m13; begin()[14] = m14; begin()[15] = m15;
    }
 
    inline Matrix& operator=( const Matrix& other)
    {
        Matrix_struct<T,ROW,COL>::operator=( other);
        return *this;
    }
 
    inline T& operator()( Size row, Size col)
    {
        mi_math_assert_msg( row < ROW, "precondition");
        mi_math_assert_msg( col < COL, "precondition");
        return begin()[row * COL + col];
    }
 
    inline const T& operator()( Size row, Size col) const
    {
        mi_math_assert_msg( row < ROW, "precondition");
        mi_math_assert_msg( col < COL, "precondition");
        return begin()[row * COL + col];
    }
 
    inline T get( Size i) const
    {
        mi_math_assert_msg( i < ROW*COL, "precondition");
        return begin()[i];
    }
 
    inline T get( Size row, Size col) const
    {
        mi_math_assert_msg( row < ROW, "precondition");
        mi_math_assert_msg( col < COL, "precondition");
        return begin()[row * COL + col];
    }
 
    inline void set( Size i, T value)
    {
        mi_math_assert_msg( i < ROW*COL, "precondition");
        begin()[i] = value;
    }
 
    inline void set( Size row, Size col, T value)
    {
        mi_math_assert_msg( row < ROW, "precondition");
        mi_math_assert_msg( col < COL, "precondition");
        begin()[row * COL + col] = value;
    }
 
    inline T det33() const
    {
        mi_static_assert( (ROW==3 || ROW==4) && (COL==3 || COL==4) );
        return this->xx * this->yy * this->zz
             + this->xy * this->yz * this->zx
             + this->xz * this->yx * this->zy
             - this->xx * this->zy * this->yz
             - this->xy * this->zz * this->yx
             - this->xz * this->zx * this->yy;
    }
 
    inline bool invert();
 
    inline void transpose()
    {
        mi_static_assert( ROW==COL);
        for( Size i=0; i < ROW-1; ++i) {
            for( Size j=i+1; j < COL; ++j) {
                T tmp = get(i,j);
                set(i,j, get(j,i));
                set(j,i, tmp);
            }
        }
    }
 
    inline void translate( T x, T y, T z)
    {
        this->wx += x;
        this->wy += y;
        this->wz += z;
    }
 
    inline void translate( const Vector<Float32,3>& vector)
    {
        this->wx += T( vector.x);
        this->wy += T( vector.y);
        this->wz += T( vector.z);
    }
 
    inline void translate( const Vector<Float64,3>& vector)
    {
        this->wx += T( vector.x);
        this->wy += T( vector.y);
        this->wz += T( vector.z);
    }
 
    inline void set_translation( T dx, T dy, T dz)
    {
        this->wx = dx;
        this->wy = dy;
        this->wz = dz;
    }
 
    inline void set_translation( const Vector<Float32,3>& vector)
    {
        this->wx = T( vector.x);
        this->wy = T( vector.y);
        this->wz = T( vector.z);
    }
 
    inline void set_translation( const Vector<Float64,3>& vector)
    {
        this->wx = T( vector.x);
        this->wy = T( vector.y);
        this->wz = T( vector.z);
    }
 
    inline void rotate( T xangle, T yangle, T zangle)
    {
        Matrix<T,4,4> tmp( T( 1));
        tmp.set_rotation( xangle, yangle, zangle);
        (*this) *= tmp;
    }
 
    inline void rotate( const Vector<Float32,3>& angles)
    {
        Matrix<T,4,4> tmp( T( 1));
        tmp.set_rotation( T( angles.x), T( angles.y), T( angles.z));
        (*this) *= tmp;
    }
 
    inline void rotate( const Vector<Float64,3>& angles)
    {
        Matrix<T,4,4> tmp( T( 1));
        tmp.set_rotation( T( angles.x), T( angles.y), T( angles.z));
        (*this) *= tmp;
    }
 
    inline void set_rotation( T x_angle, T y_angle, T z_angle);
 
    inline void set_rotation( const Vector<Float32,3>& angles)
    {
        set_rotation( T( angles.x), T( angles.y), T( angles.z));
    }
 
    inline void set_rotation( const Vector<Float64,3>& angles)
    {
        set_rotation( T( angles.x), T( angles.y), T( angles.z));
    }
 
    inline void set_rotation( const Vector<Float32,3>& axis, Float64 angle);
 
    inline void set_rotation( const Vector<Float64,3>& axis, Float64 angle);
 
    inline void lookat(
        const Vector<Float32,3>& position,
        const Vector<Float32,3>& target,
        const Vector<Float32,3>& up);
 
    inline void lookat(
        const Vector<Float64,3>& position,
        const Vector<Float64,3>& target,
        const Vector<Float64,3>& up);
};
 
 
//------ Free comparison operators ==, !=, <, <=, >, >= for matrices -----------
 
template <typename T, Size ROW, Size COL>
inline bool operator==(
    const Matrix<T,ROW,COL>& lhs,
    const Matrix<T,ROW,COL>& rhs)
{
    return is_equal( lhs, rhs);
}
 
template <typename T, Size ROW, Size COL>
inline bool operator!=(
    const Matrix<T,ROW,COL>& lhs,
    const Matrix<T,ROW,COL>& rhs)
{
    return is_not_equal( lhs, rhs);
}
 
template <typename T, Size ROW, Size COL>
inline bool operator<(
    const Matrix<T,ROW,COL>& lhs,
    const Matrix<T,ROW,COL>& rhs)
{
    return lexicographically_less( lhs, rhs);
}
 
template <typename T, Size ROW, Size COL>
inline bool operator<=(
    const Matrix<T,ROW,COL>& lhs,
    const Matrix<T,ROW,COL>& rhs)
{
    return lexicographically_less_or_equal( lhs, rhs);
}
 
template <typename T, Size ROW, Size COL>
inline bool operator>(
    const Matrix<T,ROW,COL>& lhs,
    const Matrix<T,ROW,COL>& rhs)
{
    return lexicographically_greater( lhs, rhs);
}
 
template <typename T, Size ROW, Size COL>
inline bool operator>=(
    const Matrix<T,ROW,COL>& lhs,
    const Matrix<T,ROW,COL>& rhs)
{
    return lexicographically_greater_or_equal( lhs, rhs);
}
 
//------ Operator declarations for Matrix -------------------------------------
 
template <typename T, Size ROW, Size COL>
Matrix<T,ROW,COL>& operator+=( Matrix<T,ROW,COL>& lhs, const Matrix<T,ROW,COL>& rhs);
 
template <typename T, Size ROW, Size COL>
Matrix<T,ROW,COL>& operator-=( Matrix<T,ROW,COL>& lhs, const Matrix<T,ROW,COL>& rhs);
 
template <typename T, Size ROW, Size COL>
inline Matrix<T,ROW,COL> operator+(
    const Matrix<T,ROW,COL>& lhs,
    const Matrix<T,ROW,COL>& rhs)
{
    Matrix<T,ROW,COL> temp( lhs);
    temp += rhs;
    return temp;
}
 
template <typename T, Size ROW, Size COL>
inline Matrix<T,ROW,COL> operator-(
    const Matrix<T,ROW,COL>& lhs,
    const Matrix<T,ROW,COL>& rhs)
{
    Matrix<T,ROW,COL> temp( lhs);
    temp -= rhs;
    return temp;
}
 
template <typename T, Size ROW, Size COL>
inline Matrix<T,ROW,COL> operator-(
    const Matrix<T,ROW,COL>& mat)
{
    Matrix<T,ROW,COL> temp;
    for( Size i(0u); i < ROW*COL; ++i)
        temp.begin()[i] = -mat.get(i);
    return temp;
}
 
template <typename T, Size ROW, Size COL>
inline Matrix<T,ROW,COL>& operator*=(
    Matrix<T,ROW,COL>&       lhs,
    const Matrix<T,COL,COL>& rhs)
{
    // There are more efficient ways of implementing this. Its a default solution. Specializations
    // exist below.
    Matrix<T,ROW,COL> old( lhs);
    for( Size rrow = 0; rrow < ROW; ++rrow) {
        for( Size rcol = 0; rcol < COL; ++rcol) {
            lhs( rrow, rcol) = T(0);
            for( Size k = 0; k < COL; ++k)
                lhs( rrow, rcol) += old( rrow, k) * rhs( k, rcol);
        }
    }
    return lhs;
}
 
template <typename T, Size ROW1, Size COL1, Size ROW2, Size COL2>
inline Matrix<T,ROW1,COL2> operator*(
    const Matrix<T,ROW1,COL1>& lhs,
    const Matrix<T,ROW2,COL2>& rhs)
{
    // There are more efficient ways of implementing this. Its a default solution. Specializations
    // exist below.
    mi_static_assert( COL1 == ROW2);
    Matrix<T,ROW1,COL2> result;
    for( Size rrow = 0; rrow < ROW1; ++rrow) {
        for( Size rcol = 0; rcol < COL2; ++rcol) {
            result( rrow, rcol) = T(0);
            for( Size k = 0; k < COL1; ++k)
                result( rrow, rcol) += lhs( rrow, k) * rhs( k, rcol);
        }
    }
    return result;
}
 
template <typename T, Size ROW, Size COL, Size DIM>
inline Vector<T,ROW> operator*(
    const Matrix<T,ROW,COL>& mat,
    const Vector<T,DIM>&     vec)
{
    mi_static_assert( COL == DIM);
    Vector<T,ROW> result;
    for( Size row = 0; row < ROW; ++row) {
        result[row] = T(0);
        for( Size col = 0; col < COL; ++col)
            result[row] += mat( row, col) * vec[col];
    }
    return result;
}
 
template <Size DIM, typename T, Size ROW, Size COL>
inline Vector<T,COL> operator*(
    const Vector<T,DIM>&     vec,
    const Matrix<T,ROW,COL>& mat)
{
    mi_static_assert( DIM == ROW);
    Vector<T,COL> result;
    for( Size col = 0; col < COL; ++col) {
        result[col] = T(0);
        for( Size row = 0; row < ROW; ++row)
            result[col] += mat( row, col) * vec[row];
    }
    return result;
}
 
template <typename T, Size ROW, Size COL>
inline Matrix<T,ROW,COL>& operator*=( Matrix<T,ROW,COL>& mat, T factor)
{
    for( Size i=0; i < ROW*COL; ++i)
        mat.begin()[i] *= factor;
    return mat;
}
 
template <typename T, Size ROW, Size COL>
inline Matrix<T,ROW,COL> operator*( const Matrix<T,ROW,COL>& mat, T factor)
{
    Matrix<T,ROW,COL> temp( mat);
    temp *= factor;
    return temp;
}
 
template <typename T, Size ROW, Size COL>
inline Matrix<T,ROW,COL> operator*( T factor, const Matrix<T,ROW,COL>& mat)
{
    Matrix<T,ROW,COL> temp( mat);
    temp *= factor; // * on T should be commutative (IEEE-754)
    return temp;
}
 
 
//------ Free Functions for Matrix --------------------------------------------
 
template <Size NEW_ROW, Size NEW_COL, typename T, Size ROW, Size COL>
inline Matrix<T,NEW_ROW,NEW_COL> sub_matrix(
    const Matrix<T,ROW,COL>& mat)
{
    mi_static_assert( NEW_ROW <= ROW);
    mi_static_assert( NEW_COL <= COL);
    Matrix<T,NEW_ROW,NEW_COL> result;
    for( Size i=0; i < NEW_ROW; ++i)
        for( Size j=0; j < NEW_COL; ++j)
            result( i, j) = mat( i, j);
    return result;
}
 
 
 
template <typename T, Size ROW, Size COL>
inline Matrix<T,COL,ROW> transpose(
    const Matrix<T,ROW,COL>& mat)
{
    Matrix<T,COL,ROW> result;
    for( Size i=0; i < ROW; ++i)
        for( Size j=0; j < COL; ++j)
            result( j, i) = mat( i, j);
    return result;
}
 
template <typename T, typename U>
inline U transform_point(
    const Matrix<T,4,4>& mat,    
    const U&             point)  
{
    const T w = T(mat.xw * point + mat.ww);
    if( w == T(0) || w == T(1))
        return U(mat.xx * point + mat.wx);
    else
        return U((mat.xx * point + mat.wx) / w);
}
 
template <typename T, typename U>
inline Vector<U,2> transform_point(
    const Matrix<T,4,4>& mat,    
    const Vector<U,2>&   point)  
{
    T w = T(mat.xw * point.x + mat.yw * point.y + mat.ww);
    if( w == T(0) || w == T(1))
        return Vector<U, 2>(
            U(mat.xx * point.x + mat.yx * point.y + mat.wx),
            U(mat.xy * point.x + mat.yy * point.y + mat.wy));
    else {
        w = T(1)/w; // optimization
        return Vector<U, 2>(
            U((mat.xx * point.x + mat.yx * point.y + mat.wx) * w),
            U((mat.xy * point.x + mat.yy * point.y + mat.wy) * w));
    }
}
 
template <typename T, typename U>
inline Vector<U,3> transform_point(
    const Matrix<T,4,3>& mat,    
    const Vector<U,3>&   point)  
{
    return Vector<U,3>(
        U(mat.xx * point.x + mat.yx * point.y + mat.zx * point.z + mat.wx),
        U(mat.xy * point.x + mat.yy * point.y + mat.zy * point.z + mat.wy),
        U(mat.xz * point.x + mat.yz * point.y + mat.zz * point.z + mat.wz));
}
 
template <typename T, typename U>
inline Vector<U,3> transform_point(
    const Matrix<T,4,4>& mat,    
    const Vector<U,3>&   point)  
{
    T w = T(mat.xw * point.x + mat.yw * point.y + mat.zw * point.z + mat.ww);
    if( w == T(0) || w == T(1)) // avoids temporary and division
        return Vector<U,3>(
            U(mat.xx * point.x + mat.yx * point.y + mat.zx * point.z + mat.wx),
            U(mat.xy * point.x + mat.yy * point.y + mat.zy * point.z + mat.wy),
            U(mat.xz * point.x + mat.yz * point.y + mat.zz * point.z + mat.wz));
    else {
        w = T(1)/w; // optimization
        return Vector<U,3>(
            U((mat.xx * point.x + mat.yx * point.y + mat.zx * point.z + mat.wx) * w),
            U((mat.xy * point.x + mat.yy * point.y + mat.zy * point.z + mat.wy) * w),
            U((mat.xz * point.x + mat.yz * point.y + mat.zz * point.z + mat.wz) * w));
    }
}
 
template <typename T, typename U>
inline Vector<U,4> transform_point(
    const Matrix<T,4,4>& mat,    
    const Vector<U,4>&   point)  
{
    return Vector<U, 4>(
        U(mat.xx * point.x + mat.yx * point.y + mat.zx * point.z + mat.wx * point.w),
        U(mat.xy * point.x + mat.yy * point.y + mat.zy * point.z + mat.wy * point.w),
        U(mat.xz * point.x + mat.yz * point.y + mat.zz * point.z + mat.wz * point.w),
        U(mat.xw * point.x + mat.yw * point.y + mat.zw * point.z + mat.ww * point.w));
}
 
template <typename T, typename U>
inline U transform_vector(
    const Matrix<T,4,4>& mat,    
    const U&             vector) 
{
    return U(mat.xx * vector);
}
 
template <typename T, typename U>
inline Vector<U,2> transform_vector(
    const Matrix<T,4,4>& mat,    
    const Vector<U,2>&   vector) 
{
    return Vector<U,2>(
        U(mat.xx * vector.x + mat.yx * vector.y),
        U(mat.xy * vector.x + mat.yy * vector.y));
}
 
template <typename T, typename U>
inline Vector<U,3> transform_vector(
    const Matrix<T,3,3>& mat,    
    const Vector<U,3>&   vector) 
{
    return Vector<U,3>(
        U(mat.xx * vector.x + mat.yx * vector.y + mat.zx * vector.z),
        U(mat.xy * vector.x + mat.yy * vector.y + mat.zy * vector.z),
        U(mat.xz * vector.x + mat.yz * vector.y + mat.zz * vector.z));
}
 
template <typename T, typename U>
inline Vector<U,3> transform_vector(
    const Matrix<T,4,3>& mat,    
    const Vector<U,3>&   vector) 
{
    return Vector<U,3>(
        U(mat.xx * vector.x + mat.yx * vector.y + mat.zx * vector.z),
        U(mat.xy * vector.x + mat.yy * vector.y + mat.zy * vector.z),
        U(mat.xz * vector.x + mat.yz * vector.y + mat.zz * vector.z));
}
 
template <typename T, typename U>
inline Vector<U,3> transform_vector(
    const Matrix<T,4,4>& mat,    
    const Vector<U,3>&   vector) 
{
    return Vector<U,3>(
        U(mat.xx * vector.x + mat.yx * vector.y + mat.zx * vector.z),
        U(mat.xy * vector.x + mat.yy * vector.y + mat.zy * vector.z),
        U(mat.xz * vector.x + mat.yz * vector.y + mat.zz * vector.z));
}
 
template <typename T, typename U>
inline Vector<U,3> transform_normal_inv(
    const Matrix<T,3,3>& inv_mat, 
    const Vector<U,3>&   normal)  
{
    return Vector<U,3>(
        U(inv_mat.xx * normal.x + inv_mat.xy * normal.y + inv_mat.xz * normal.z),
        U(inv_mat.yx * normal.x + inv_mat.yy * normal.y + inv_mat.yz * normal.z),
        U(inv_mat.zx * normal.x + inv_mat.zy * normal.y + inv_mat.zz * normal.z));
}
 
template <typename T, typename U>
inline Vector<U,3> transform_normal_inv(
    const Matrix<T,4,4>& inv_mat, 
    const Vector<U,3>&   normal)  
{
    return Vector<U,3>(
        U(inv_mat.xx * normal.x + inv_mat.xy * normal.y + inv_mat.xz * normal.z),
        U(inv_mat.yx * normal.x + inv_mat.yy * normal.y + inv_mat.yz * normal.z),
        U(inv_mat.zx * normal.x + inv_mat.zy * normal.y + inv_mat.zz * normal.z));
}
 
template <typename T, typename U>
inline Vector<U,3> transform_normal(
    const Matrix<T,4,4>& mat,    
    const Vector<U,3>&   normal) 
{
    Matrix<T,3,3> sub_mat( mat.xx, mat.xy, mat.xz,
                           mat.yx, mat.yy, mat.yz,
                           mat.zx, mat.zy, mat.zz);
    bool inverted = sub_mat.invert();
    if( !inverted)
        return normal;
    return Vector<U,3>(
        U(sub_mat.xx * normal.x + sub_mat.xy * normal.y + sub_mat.xz * normal.z),
        U(sub_mat.yx * normal.x + sub_mat.yy * normal.y + sub_mat.yz * normal.z),
        U(sub_mat.zx * normal.x + sub_mat.zy * normal.y + sub_mat.zz * normal.z));
}
 
 
//------ Definitions of non-inline function -----------------------------------
 
template <typename T, Size ROW, Size COL>
Matrix<T,ROW,COL>& operator+=(
    Matrix<T,ROW,COL>&       lhs,
    const Matrix<T,ROW,COL>& rhs)
{
    for( Size i=0; i < ROW*COL; ++i)
        lhs.begin()[i] += rhs.get(i);
    return lhs;
}
 
template <typename T, Size ROW, Size COL>
Matrix<T,ROW,COL>& operator-=(
    Matrix<T,ROW,COL>&       lhs,
    const Matrix<T,ROW,COL>& rhs)
{
    for( Size i=0; i < ROW*COL; ++i)
        lhs.begin()[i] -= rhs.get(i);
    return lhs;
}
 
#ifndef MI_FOR_DOXYGEN_ONLY
 
template <typename T, Size ROW, Size COL>
inline void Matrix<T,ROW,COL>::set_rotation( T xangle, T yangle, T zangle)
{
    mi_static_assert( COL == 4 && ROW == 4);
    T tsx, tsy, tsz; // sine of [xyz]_angle
    T tcx, tcy, tcz; // cosine of [xyz]_angle
    T tmp;
    const T min_angle = T(0.00024f);
 
    if( abs( xangle) > min_angle) {
        tsx = sin( xangle);
        tcx = cos( xangle);
    } else {
        tsx = xangle;
        tcx = T(1);
    }
    if( abs( yangle) > min_angle) {
        tsy = sin( yangle);
        tcy = cos( yangle);
    } else {
        tsy = yangle;
        tcy = T(1);
    }
    if( abs(zangle) > min_angle) {
        tsz = sin( zangle);
        tcz = cos( zangle);
    } else {
        tsz = T(zangle);
        tcz = T(1);
    }
    this->xx = tcy * tcz;
    this->xy = tcy * tsz;
    this->xz = -tsy;
 
    tmp  = tsx * tsy;
    this->yx = tmp * tcz - tcx * tsz;
    this->yy = tmp * tsz + tcx * tcz;
    this->yz = tsx * tcy;
 
    tmp  = tcx * tsy;
    this->zx = tmp * tcz + tsx * tsz;
    this->zy = tmp * tsz - tsx * tcz;
    this->zz = tcx * tcy;
}
 
template <typename T, Size ROW, Size COL>
inline void Matrix<T,ROW,COL>::set_rotation( const Vector<Float32,3>& axis_v, Float64 angle)
{
    mi_static_assert( COL == 4 && ROW == 4);
    Vector<T,3> axis( axis_v);
    const T min_angle = T(0.00024f);
 
    if( abs( T(angle)) < min_angle) {
        T xa = axis.x * T(angle);
        T ya = axis.y * T(angle);
        T za = axis.z * T(angle);
 
        this->xx = T(1);
        this->xy = za;
        this->xz = -ya;
        this->xw = T(0);
 
        this->yx = za;
        this->yy = T(1);
        this->yz = xa;
        this->yw = T(0);
 
        this->zx = -ya;
        this->zy = -xa;
        this->zz = T(1);
        this->zw = T(0);
    } else {
        T s = sin( T(angle));
        T c = cos( T(angle));
        T t = T(1) - c;
        T tmp;
 
        tmp = t * T(axis.x);
        this->xx  = tmp * T(axis.x) + c;
        this->xy  = tmp * T(axis.y) + s * T(axis.z);
        this->xz  = tmp * T(axis.z) - s * T(axis.y);
        this->xw  = T(0);
 
        tmp = t * T(axis.y);
        this->yx  = tmp * T(axis.x) - s * T(axis.z);
        this->yy  = tmp * T(axis.y) + c;
        this->yz  = tmp * T(axis.z) + s * T(axis.x);
        this->yw  = T(0);
 
        tmp = t * T(axis.z);
        this->zx  = tmp * T(axis.x) + s * T(axis.y);
        this->zy  = tmp * T(axis.y) - s * T(axis.x);
        this->zz  = tmp * T(axis.z) + c;
        this->zw  = T(0);
    }
    this->wx = this->wy = this->wz = T(0);
    this->ww = T(1);
}
 
template <typename T, Size ROW, Size COL>
inline void Matrix<T,ROW,COL>::set_rotation( const Vector<Float64,3>& axis_v, Float64 angle)
{
    mi_static_assert( COL == 4 && ROW == 4);
    Vector<T,3> axis( axis_v);
    const T min_angle = T(0.00024f);
 
    if( abs(T(angle)) < min_angle) {
        T xa = axis.x * T(angle);
        T ya = axis.y * T(angle);
        T za = axis.z * T(angle);
 
        this->xx = T(1);
        this->xy = za;
        this->xz = -ya;
        this->xw = T(0);
 
        this->yx = za;
        this->yy = T(1);
        this->yz = xa;
        this->yw = T(0);
 
        this->zx = -ya;
        this->zy = -xa;
        this->zz = T(1);
        this->zw = T(0);
    } else {
        T s = sin( T(angle));
        T c = cos( T(angle));
        T t = T(1) - c;
        T tmp;
 
        tmp = t * T(axis.x);
        this->xx  = tmp * T(axis.x) + c;
        this->xy  = tmp * T(axis.y) + s * T(axis.z);
        this->xz  = tmp * T(axis.z) - s * T(axis.y);
        this->xw  = T(0);
 
        tmp = t * T(axis.y);
        this->yx  = tmp * T(axis.x) - s * T(axis.z);
        this->yy  = tmp * T(axis.y) + c;
        this->yz  = tmp * T(axis.z) + s * T(axis.x);
        this->yw  = T(0);
 
        tmp = t * T(axis.z);
        this->zx  = tmp * T(axis.x) + s * T(axis.y);
        this->zy  = tmp * T(axis.y) - s * T(axis.x);
        this->zz  = tmp * T(axis.z) + c;
        this->zw  = T(0);
    }
    this->wx = this->wy = this->wz = T(0);
    this->ww = T(1);
}
 
template <typename T, Size ROW, Size COL>
inline void Matrix<T,ROW,COL>::lookat(
    const Vector<Float32,3>& position,
    const Vector<Float32,3>& target,
    const Vector<Float32,3>& up)
{
    mi_static_assert( COL == 4 && ROW == 4);
    Vector<Float32,3> xaxis, yaxis, zaxis;
 
    // Z vector
    zaxis = position - target;
    zaxis.normalize();
 
    // X vector = up cross Z
    xaxis = cross( up, zaxis);
    xaxis.normalize();
 
    // Recompute Y = Z cross X
    yaxis = cross( zaxis, xaxis);
    yaxis.normalize();
 
    // Build rotation matrix
    Matrix<T,4,4> rot(
        T(xaxis.x), T(yaxis.x), T(zaxis.x), T(0),
        T(xaxis.y), T(yaxis.y), T(zaxis.y), T(0),
        T(xaxis.z), T(yaxis.z), T(zaxis.z), T(0),
        T(0),       T(0),       T(0),       T(1));
 
    // Compute the new position by multiplying the inverse position with the rotation matrix
    Matrix<T,4,4> trans(
        T(1),      T(0),      T(0),      T(0),
        T(0),      T(1),      T(0),      T(0),
        T(0),      T(0),      T(1),      T(0),
        T(-position.x), T(-position.y), T(-position.z), T(1));
 
    *this = trans * rot;
}
 
template <typename T, Size ROW, Size COL>
inline void Matrix<T,ROW,COL>::lookat(
    const Vector<Float64,3>& position,
    const Vector<Float64,3>& target,
    const Vector<Float64,3>& up)
{
    mi_static_assert( COL == 4 && ROW == 4);
    Vector<Float64,3> xaxis, yaxis, zaxis;
 
    // Z vector
    zaxis = position - target;
    zaxis.normalize();
 
    // X vector = up cross Z
    xaxis = cross( up, zaxis);
    xaxis.normalize();
 
    // Recompute Y = Z cross X
    yaxis = cross( zaxis, xaxis);
    yaxis.normalize();
 
    // Build rotation matrix
    Matrix<T,4,4> rot(
        T(xaxis.x), T(yaxis.x), T(zaxis.x), T(0),
        T(xaxis.y), T(yaxis.y), T(zaxis.y), T(0),
        T(xaxis.z), T(yaxis.z), T(zaxis.z), T(0),
        T(0),       T(0),       T(0),       T(1));
 
    // Compute the new position by multiplying the inverse position with the rotation matrix
    Matrix<T,4,4> trans(
        T(1),      T(0),      T(0),      T(0),
        T(0),      T(1),      T(0),      T(0),
        T(0),      T(0),      T(1),      T(0),
        T(-position.x), T(-position.y), T(-position.z), T(1));
 
    *this = trans * rot;
}
 
 
//------ Definition and helper for matrix inversion ---------------------------
 
// Matrix inversion class, specialized for symmetric matrices and low dimensions
template <class T, Size ROW, Size COL>
class Matrix_inverter
{
public:
    typedef math::Matrix<T,ROW,COL> Matrix;
 
    // Inverts the matrix \c M if possible and returns \c true.
    //
    // If the matrix cannot be inverted or if \c ROW != \c COL, this function returns \c false.
    static inline bool invert( Matrix& /* M */) { return false; }
};
 
// Matrix inversion class, specialization for symmetric matrices
template <class T, Size DIM>
class Matrix_inverter<T,DIM,DIM>
{
public:
    typedef math::Matrix<T,DIM,DIM> Matrix;
    typedef math::Vector<T,DIM>     Value_vector;
    typedef math::Vector<Size,DIM>  Index_vector;
 
    // LU decomposition of matrix lu in place.
    //
    // Returns \c false if matrix cannot be decomposed, for example, if it is not invertible, and \c
    // true otherwise. Returns also a row permutation in indx.
    static bool lu_decomposition(
        Matrix&       lu,         // matrix to decompose, modified in place
        Index_vector& indx);      // row permutation info indx[4]
 
    // Solves the equation lu * x = b for x.  lu and indx are the results of lu_decomposition. The
    // solution is returned in b.
    static void lu_backsubstitution(
        const Matrix&       lu,   // LU decomposed matrix
        const Index_vector& indx, // permutation vector
        Value_vector&       b);   // right hand side vector b, modified in place
 
    static bool invert( Matrix& mat);
};
 
template <class T, Size DIM>
bool  Matrix_inverter<T,DIM,DIM>::lu_decomposition(
    Matrix&       lu,
    Index_vector& indx)
{
    Value_vector vv;             // implicit scaling of each row
 
    for( Size i = 0; i < DIM; i++) {    // get implicit scaling
        T big = T(0);
        for( Size j = 0; j < DIM; j++) {
            T temp = abs(lu.get(i,j));
            if( temp > big)
                big = temp;
        }
        if( big == T(0))
            return false;
        vv[i] = T(1) / big;             // save scaling
    }
    //
    // loop over columns of Crout's method
    //
    Size imax = 0;
    for( Size j = 0; j < DIM; j++) {
        for( Size i = 0; i < j; i++) {
            T sum = lu.get(i,j);
            for( Size k = 0; k < i; k++)
                sum -= lu.get(i,k) * lu.get(k,j);
            lu.set(i, j, sum);
        }
        T big = 0;                      // init for pivot search
        for( Size i = j; i < DIM; i++) {
            T sum = lu.get(i,j);
            for( Size k = 0; k < j; k++)
                sum -= lu.get(i,k) * lu.get(k,j);
            lu.set(i, j, sum);
            T dum = vv[i] * abs(sum);
            if( dum >= big) {           // pivot good?
                big = dum;
                imax = i;
            }
        }
        if( j != imax) {                // interchange rows
            for( Size k = 0; k < DIM; k++) {
                T dum = lu.get(imax,k);
                lu.set(imax, k, lu.get(j,k));
                lu.set(j, k, dum);
            }
            vv[imax] = vv[j];           // interchange scale factor
        }
        indx[j] = imax;
        if( lu.get(j,j) == 0)
            return false;
        if( j != DIM-1) {               // divide by pivot element
            T dum = T(1) / lu.get(j,j);
            for( Size i = j + 1; i < DIM; i++)
                lu.set(i, j, lu.get(i,j) * dum);
        }
    }
    return true;
}
 
template <class T, Size DIM>
void Matrix_inverter<T,DIM,DIM>::lu_backsubstitution(
    const Matrix&       lu,
    const Index_vector& indx,
    Value_vector&                         b)
{
    // when ii != DIM+1, ii is index of first non-vanishing element of b
    Size ii = DIM+1;
    for( Size i = 0; i < DIM; i++) {
        Size ip = indx[i];
        T sum   = b[ip];
        b[ip]   = b[i];
        if( ii != DIM+1) {
            for( Size j = ii; j < i; j++) {
                sum -= lu.get(i,j) * b[j];
            }
        } else {
            if( sum != T(0)) {          // non-zero element
                ii = i;
            }
        }
        b[i] = sum;
    }
    for( Size i2 = DIM; i2 > 0;) {      // backsubstitution
        --i2;
        T sum = b[i2];
        for( Size j = i2+1; j < DIM; j++)
            sum -= lu.get(i2,j) * b[j];
        b[i2] = sum / lu.get(i2,i2);    // store comp. of sol. vector
    }
}
 
template <class T, Size DIM>
bool  Matrix_inverter<T,DIM,DIM>::invert( Matrix& mat)
{
    Matrix lu(mat);     // working copy of matrix to invert
    Index_vector indx;  // row permutation info
 
    // LU decompose, return if it fails
    if( !lu_decomposition(lu, indx))
        return false;
 
    // solve for each column vector of the I matrix
    for( Size j = 0; j < DIM; ++j) {
        Value_vector col(T(0)); // TODO: do that directly in the mat matrix
        col[j] = T(1);
        lu_backsubstitution( lu, indx, col);
        for( Size i = 0; i < DIM; ++i) {
            mat.set( i, j, col[i]);
        }
    }
    return true;
}
 
// Matrix inversion class, specialization for 1x1 matrix
template <class T>
class Matrix_inverter<T,1,1>
{
public:
    typedef math::Matrix<T,1,1>     Matrix;
 
    static inline bool invert( Matrix& mat)
    {
        T s = mat.get( 0, 0);
        if( s == T(0))
            return false;
        mat.set( 0, 0, T(1) / s);
        return true;
    }
};
 
// Matrix inversion class, specialization for 2x2 matrix
template <class T>
class Matrix_inverter<T,2,2>
{
public:
    typedef math::Matrix<T,2,2>        Matrix;
 
    static inline bool invert( Matrix& mat)
    {
        T a = mat.get( 0, 0);
        T b = mat.get( 0, 1);
        T c = mat.get( 1, 0);
        T d = mat.get( 1, 1);
        T det = a*d - b*c;
        if( det == T( 0))
            return false;
        T rdet = T(1) / det;
        mat.set( 0, 0, d * rdet);
        mat.set( 0, 1,-b * rdet);
        mat.set( 1, 0,-c * rdet);
        mat.set( 1, 1, a * rdet);
        return true;
    }
};
 
template <typename T, Size ROW, Size COL>
inline bool Matrix<T,ROW,COL>::invert()
{
    return Matrix_inverter<T,ROW,COL>::invert( *this);
}
 
 
 
//------ Specializations and (specialized) overloads --------------------------
 
// Specialization of common matrix multiplication for 4x4 matrices.
template <typename T>
inline Matrix<T,4,4>& operator*=(
    Matrix<T,4,4>&       lhs,
    const Matrix<T,4,4>& rhs)
{
    Matrix<T,4,4> old( lhs);
 
    lhs.xx = old.xx * rhs.xx + old.xy * rhs.yx + old.xz * rhs.zx + old.xw * rhs.wx;
    lhs.xy = old.xx * rhs.xy + old.xy * rhs.yy + old.xz * rhs.zy + old.xw * rhs.wy;
    lhs.xz = old.xx * rhs.xz + old.xy * rhs.yz + old.xz * rhs.zz + old.xw * rhs.wz;
    lhs.xw = old.xx * rhs.xw + old.xy * rhs.yw + old.xz * rhs.zw + old.xw * rhs.ww;
 
    lhs.yx = old.yx * rhs.xx + old.yy * rhs.yx + old.yz * rhs.zx + old.yw * rhs.wx;
    lhs.yy = old.yx * rhs.xy + old.yy * rhs.yy + old.yz * rhs.zy + old.yw * rhs.wy;
    lhs.yz = old.yx * rhs.xz + old.yy * rhs.yz + old.yz * rhs.zz + old.yw * rhs.wz;
    lhs.yw = old.yx * rhs.xw + old.yy * rhs.yw + old.yz * rhs.zw + old.yw * rhs.ww;
 
    lhs.zx = old.zx * rhs.xx + old.zy * rhs.yx + old.zz * rhs.zx + old.zw * rhs.wx;
    lhs.zy = old.zx * rhs.xy + old.zy * rhs.yy + old.zz * rhs.zy + old.zw * rhs.wy;
    lhs.zz = old.zx * rhs.xz + old.zy * rhs.yz + old.zz * rhs.zz + old.zw * rhs.wz;
    lhs.zw = old.zx * rhs.xw + old.zy * rhs.yw + old.zz * rhs.zw + old.zw * rhs.ww;
 
    lhs.wx = old.wx * rhs.xx + old.wy * rhs.yx + old.wz * rhs.zx + old.ww * rhs.wx;
    lhs.wy = old.wx * rhs.xy + old.wy * rhs.yy + old.wz * rhs.zy + old.ww * rhs.wy;
    lhs.wz = old.wx * rhs.xz + old.wy * rhs.yz + old.wz * rhs.zz + old.ww * rhs.wz;
    lhs.ww = old.wx * rhs.xw + old.wy * rhs.yw + old.wz * rhs.zw + old.ww * rhs.ww;
 
    return lhs;
}
 
// Specialization of common matrix multiplication for 4x4 matrices.
template <typename T>
inline Matrix<T,4,4> operator*(
    const Matrix<T,4,4>& lhs,
    const Matrix<T,4,4>& rhs)
{
    Matrix<T,4,4> temp( lhs);
    temp *= rhs;
    return temp;
}
 
#endif // MI_FOR_DOXYGEN_ONLY
 // end group mi_math_matrix
 
} // namespace math
 
} // namespace mi
 
#endif // MI_MATH_MATRIX_H