LCOV - code coverage report
Current view: top level - vmmlib - matrix.hpp (source / functions) Hit Total Coverage
Test: vmmlib Lines: 164 283 58.0 %
Date: 2018-06-05 00:11:40 Functions: 37 203 18.2 %

          Line data    Source code
       1             : /*
       2             :  * Copyright (c) 2006-2016, Visualization and Multimedia Lab,
       3             :  *                          University of Zurich <http://vmml.ifi.uzh.ch>,
       4             :  *                          Eyescale Software GmbH,
       5             :  *                          Blue Brain Project, EPFL
       6             :  *
       7             :  * This file is part of VMMLib <https://github.com/VMML/vmmlib/>
       8             :  *
       9             :  * Redistribution and use in source and binary forms, with or without
      10             :  * modification, are permitted provided that the following conditions are met:
      11             :  *
      12             :  * Redistributions of source code must retain the above copyright notice, this
      13             :  * list of conditions and the following disclaimer.  Redistributions in binary
      14             :  * form must reproduce the above copyright notice, this list of conditions and
      15             :  * the following disclaimer in the documentation and/or other materials provided
      16             :  * with the distribution.  Neither the name of the Visualization and Multimedia
      17             :  * Lab, University of Zurich nor the names of its contributors may be used to
      18             :  * endorse or promote products derived from this software without specific prior
      19             :  * written permission.
      20             :  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
      21             :  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
      22             :  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
      23             :  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
      24             :  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
      25             :  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
      26             :  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
      27             :  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
      28             :  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
      29             :  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
      30             :  * POSSIBILITY OF SUCH DAMAGE.
      31             :  */
      32             : 
      33             : #ifndef __VMML__MATRIX__HPP__
      34             : #define __VMML__MATRIX__HPP__
      35             : 
      36             : #include <vmmlib/types.hpp>
      37             : 
      38             : #include <algorithm>
      39             : #include <cmath>
      40             : #include <cstring>
      41             : #include <iomanip>
      42             : #include <iostream>
      43             : #include <limits>
      44             : #include <stdexcept>
      45             : #include <type_traits>
      46             : #include <vector>
      47             : 
      48             : namespace vmml
      49             : {
      50             : /** Matrix with R rows and C columns of element type T */
      51             : template <size_t R, size_t C, typename T>
      52             : class Matrix
      53             : {
      54             : public:
      55             :     /**
      56             :      * Construct a zero-initialized matrix.
      57             :      * Square matrices are initialized as identity.
      58             :      */
      59             :     Matrix();
      60             : 
      61             :     /**
      62             :      * Construct a matrix with default values.
      63             :      * Missing data is zero-initialized. Additional data is ignored.
      64             :      */
      65             :     Matrix(const T* begin, const T* end);
      66             : 
      67             :     /**
      68             :      * Construct a matrix with default values.
      69             :      * Missing data is zero-initialized. Additional data is ignored.
      70             :      */
      71             :     explicit Matrix(const std::vector<T>& data);
      72             : 
      73             :     /**
      74             :      * Copy-construct a matrix.
      75             :      * Missing data is zero-initialized. Additional data is ignored.
      76             :      */
      77             :     template <size_t P, size_t Q>
      78             :     Matrix(const Matrix<P, Q, T>& source);
      79             : 
      80             :     /**
      81             :      * Construct a new 4x4 transformation matrix from a rotation quaternion and
      82             :      * a translation vector.
      83             :      */
      84             :     template <size_t S>
      85             :     Matrix(
      86             :         const Quaternion<T>& rotation, const vector<S, T>& translation,
      87             :         typename std::enable_if<R == S + 1 && C == S + 1 && S == 3>::type* = 0);
      88             : 
      89             :     /**
      90             :      * Construct a new transformation matrix from an eye position, lookat
      91             :      * position and up vector, following convention from gluLookAt().
      92             :      */
      93             :     template <size_t S>
      94             :     Matrix(
      95             :         const vector<S, T>& eye, const vector<S, T>& lookat,
      96             :         const vector<S, T>& up,
      97             :         typename std::enable_if<R == S + 1 && C == S + 1 && S == 3>::type* = 0);
      98             : 
      99             :     /** @name matrix-matrix operations */
     100             :     //@{
     101             :     /** @return true if both matrices are equal. */
     102             :     bool operator==(const Matrix& other) const;
     103             : 
     104             :     /** @return true if both matrices are not equal. */
     105             :     bool operator!=(const Matrix& other) const;
     106             : 
     107             :     /** @return true if both matrices are equal within the given tolerance. */
     108             :     bool equals(const Matrix& other,
     109             :                 T tolerance = std::numeric_limits<T>::epsilon()) const;
     110             : 
     111             :     /** Set this to the product of the two matrices (left_RxP * right_PxC) */
     112             :     template <size_t P>
     113             :     Matrix<R, C, T>& multiply(const Matrix<R, P, T>& left,
     114             :                               const Matrix<P, C, T>& right);
     115             : 
     116             :     /** @return matrix_RxP = (this) matrix * other matrix_CxP; */
     117             :     template <size_t P>
     118             :     Matrix<R, P, T> operator*(const Matrix<C, P, T>& other) const;
     119             : 
     120             :     /** Multiply two square matrices */
     121             :     template <size_t O, size_t P, typename = typename std::enable_if<
     122             :                                       R == C && O == P && R == O>::type>
     123             :     Matrix<R, C, T>& operator*=(const Matrix<O, P, T>& right);
     124             : 
     125             :     /** Element-wise addition of two matrices */
     126             :     Matrix operator+(const Matrix& other) const;
     127             : 
     128             :     /** Element-wise substraction of two matrices */
     129             :     Matrix operator-(const Matrix& other) const;
     130             : 
     131             :     /** Element-wise addition of two matrices */
     132             :     void operator+=(const Matrix& other);
     133             : 
     134             :     /** Element-wise substraction of two matrices */
     135             :     void operator-=(const Matrix& other);
     136             :     //@}
     137             : 
     138             :     /** @name matrix-vector operations */
     139             :     //@{
     140             :     /** Transform column vector by matrix ( res = matrix * vec ) */
     141             :     vector<R, T> operator*(const vector<C, T>& other) const;
     142             :     //@}
     143             : 
     144             :     /** @name Data access */
     145             :     //@{
     146             :     /** @return the element at the given row and column. */
     147             :     T& operator()(size_t rowIndex, size_t colIndex);
     148             : 
     149             :     /** @return the element at the given row and column. */
     150             :     T operator()(size_t rowIndex, size_t colIndex) const;
     151             : 
     152             :     /** @return the pointer to the data storage in column-major order. */
     153           0 :     const T* data() const { return array; }
     154             :     /** @return the sub matrix of size OxP at the given start indices. */
     155             :     template <size_t O, size_t P>
     156             :     Matrix<O, P, T> getSubMatrix(
     157             :         size_t rowOffset, size_t colOffset,
     158             :         typename std::enable_if<O <= R && P <= C>::type* = 0) const;
     159             : 
     160             :     /** Set the sub matrix of size OxP at the given start indices. */
     161             :     template <size_t O, size_t P>
     162             :     typename std::enable_if<O <= R && P <= C>::type* setSubMatrix(
     163             :         const Matrix<O, P, T>& sub_matrix, size_t rowOffset, size_t colOffset);
     164             : 
     165             :     /** Assign the given matrix. */
     166             :     const Matrix& operator=(const Matrix<R, C, T>& source_);
     167             : 
     168             :     /**
     169             :      * Assign the given matrix.
     170             :      *
     171             :      * Remaining data is zero-filled. Additional data is ignored.
     172             :      */
     173             :     template <size_t P, size_t Q>
     174             :     const Matrix& operator=(const Matrix<P, Q, T>& source_);
     175             : 
     176             :     /**
     177             :      * Assign the given data.
     178             :      *
     179             :      * Remaining data is zero-filled. Additional data is ignored.
     180             :      */
     181             :     void operator=(const std::vector<T>& data);
     182             : 
     183             :     /** @return the negated matrix of this matrix. */
     184             :     Matrix<R, C, T> operator-() const;
     185             : 
     186             :     /** @return a vector of the given column. */
     187             :     vector<R, T> getColumn(size_t columnIndex) const;
     188             : 
     189             :     /** Set the given column. */
     190             :     void setColumn(size_t index, const vector<R, T>& column);
     191             : 
     192             :     /** @return a vector of the given row. */
     193             :     vector<C, T> getRow(size_t index) const;
     194             : 
     195             :     /** Set the given row. */
     196             :     void setRow(size_t index, const vector<C, T>& row);
     197             : 
     198             :     /** @return the translation vector (of a 3x3 or 4x4 matrix) */
     199             :     vector<C - 1, T> getTranslation() const;
     200             : 
     201             :     /** Set the translation vector (of a 3x3 or 4x4 matrix) */
     202             :     Matrix<R, C, T>& setTranslation(const vector<C - 1, T>& t);
     203             : 
     204             :     /**
     205             :      * Decompose a 4x4 transformation matrix to eye position, lookAt position
     206             :      * and up vector.
     207             :      */
     208             :     template <size_t S>
     209             :     void getLookAt(vector<S, T>& eye, vector<S, T>& lookAt, vector<S, T>& up,
     210             :                    typename std::enable_if<R == S + 1 && C == S + 1 &&
     211             :                                            S == 3>::type* = 0) const;
     212             :     //@}
     213             : 
     214             :     /**
     215             :      * Compute and return the inverted matrix of this matrix.
     216             :      *
     217             :      * Sets values to quiet_NaN if not invertible.
     218             :      */
     219             :     Matrix<R, C, T> inverse() const;
     220             : 
     221             :     template <size_t O, size_t P>
     222             :     typename std::enable_if<O == P && R == C && O == R && R >= 2>::type*
     223             :         getAdjugate(Matrix<O, P, T>& adjugate) const;
     224             : 
     225             :     template <size_t O, size_t P>
     226             :     typename std::enable_if<O == P && R == C && O == R && R >= 2>::type*
     227             :         getCofactors(Matrix<O, P, T>& cofactors) const;
     228             : 
     229             :     // returns the determinant of a square matrix R-1, C-1
     230             :     template <size_t O, size_t P>
     231             :     T getMinor(Matrix<O, P, T>& minor_, size_t row_to_cut, size_t col_to_cut,
     232             :                typename std::enable_if<O == R - 1 && P == C - 1 && R == C &&
     233             :                                        R >= 2>::type* = 0) const;
     234             : 
     235             :     /** @name Transformations on 4*4 matrices */
     236             :     //@{
     237             :     template <typename TT>
     238             :     Matrix<R, C, T>& rotate_x(
     239             :         TT angle, typename std::enable_if<R == C && R == 4, TT>::type* = 0);
     240             : 
     241             :     template <typename TT>
     242             :     Matrix<R, C, T>& rotate_y(
     243             :         TT angle, typename std::enable_if<R == C && R == 4, TT>::type* = 0);
     244             : 
     245             :     template <typename TT>
     246             :     Matrix<R, C, T>& rotate_z(
     247             :         TT angle, typename std::enable_if<R == C && R == 4, TT>::type* = 0);
     248             : 
     249             :     template <typename TT>
     250             :     Matrix<R, C, T>& pre_rotate_x(
     251             :         TT angle, typename std::enable_if<R == C && R == 4, TT>::type* = 0);
     252             : 
     253             :     template <typename TT>
     254             :     Matrix<R, C, T>& pre_rotate_y(
     255             :         TT angle, typename std::enable_if<R == C && R == 4, TT>::type* = 0);
     256             : 
     257             :     template <typename TT>
     258             :     Matrix<R, C, T>& pre_rotate_z(
     259             :         TT angle, typename std::enable_if<R == C && R == 4, TT>::type* = 0);
     260             : 
     261             :     template <typename TT>
     262             :     Matrix<R, C, T>& scale(
     263             :         const vector<3, TT>& scale_,
     264             :         typename std::enable_if<R == C && R == 4, TT>::type* = 0);
     265             : 
     266             :     template <typename TT>
     267             :     Matrix<R, C, T>& scaleTranslation(
     268             :         const vector<3, TT>& scale_,
     269             :         typename std::enable_if<R == C && R == 4, TT>::type* = 0);
     270             :     //@}
     271             : 
     272           0 :     friend std::ostream& operator<<(std::ostream& os,
     273             :                                     const Matrix<R, C, T>& matrix)
     274             :     {
     275           0 :         const std::ios::fmtflags flags = os.flags();
     276           0 :         const int prec = os.precision();
     277             : 
     278           0 :         os.setf(std::ios::right, std::ios::adjustfield);
     279           0 :         os.precision(7);
     280             : 
     281           0 :         for (size_t rowIndex = 0; rowIndex < R; ++rowIndex)
     282             :         {
     283           0 :             os << "|";
     284           0 :             for (size_t colIndex = 0; colIndex < C; ++colIndex)
     285           0 :                 os << std::setw(10) << matrix(rowIndex, colIndex) << " ";
     286           0 :             os << "|" << std::endl;
     287             :         }
     288           0 :         os.precision(prec);
     289           0 :         os.setf(flags);
     290           0 :         return os;
     291             :     };
     292             : 
     293             :     T array[R * C]; //!< column by column storage
     294             : };
     295             : }
     296             : 
     297             : #include <vmmlib/quaternion.hpp>
     298             : #include <vmmlib/vector.hpp>
     299             : 
     300             : namespace vmml
     301             : {
     302             : /** @name Free matrix functions */
     303             : //@{
     304             : template <typename T>
     305           0 : inline T computeDeterminant(const Matrix<1, 1, T>& matrix_)
     306             : {
     307           0 :     return matrix_.array[0];
     308             : }
     309             : 
     310             : template <typename T>
     311           1 : inline T computeDeterminant(const Matrix<2, 2, T>& matrix_)
     312             : {
     313           1 :     return matrix_(0, 0) * matrix_(1, 1) - matrix_(0, 1) * matrix_(1, 0);
     314             : }
     315             : 
     316             : template <typename T>
     317             : inline T computeDeterminant(const Matrix<3, 3, T>& m_)
     318             : {
     319             :     return m_(0, 0) * (m_(1, 1) * m_(2, 2) - m_(1, 2) * m_(2, 1)) +
     320             :            m_(0, 1) * (m_(1, 2) * m_(2, 0) - m_(1, 0) * m_(2, 2)) +
     321             :            m_(0, 2) * (m_(1, 0) * m_(2, 1) - m_(1, 1) * m_(2, 0));
     322             : }
     323             : 
     324             : template <typename T>
     325             : inline T computeDeterminant(const Matrix<4, 4, T>& m)
     326             : {
     327             :     return m(0, 3) * m(1, 2) * m(2, 1) * m(3, 0) -
     328             :            m(0, 2) * m(1, 3) * m(2, 1) * m(3, 0) -
     329             :            m(0, 3) * m(1, 1) * m(2, 2) * m(3, 0) +
     330             :            m(0, 1) * m(1, 3) * m(2, 2) * m(3, 0) +
     331             :            m(0, 2) * m(1, 1) * m(2, 3) * m(3, 0) -
     332             :            m(0, 1) * m(1, 2) * m(2, 3) * m(3, 0) -
     333             :            m(0, 3) * m(1, 2) * m(2, 0) * m(3, 1) +
     334             :            m(0, 2) * m(1, 3) * m(2, 0) * m(3, 1) +
     335             :            m(0, 3) * m(1, 0) * m(2, 2) * m(3, 1) -
     336             :            m(0, 0) * m(1, 3) * m(2, 2) * m(3, 1) -
     337             :            m(0, 2) * m(1, 0) * m(2, 3) * m(3, 1) +
     338             :            m(0, 0) * m(1, 2) * m(2, 3) * m(3, 1) +
     339             :            m(0, 3) * m(1, 1) * m(2, 0) * m(3, 2) -
     340             :            m(0, 1) * m(1, 3) * m(2, 0) * m(3, 2) -
     341             :            m(0, 3) * m(1, 0) * m(2, 1) * m(3, 2) +
     342             :            m(0, 0) * m(1, 3) * m(2, 1) * m(3, 2) +
     343             :            m(0, 1) * m(1, 0) * m(2, 3) * m(3, 2) -
     344             :            m(0, 0) * m(1, 1) * m(2, 3) * m(3, 2) -
     345             :            m(0, 2) * m(1, 1) * m(2, 0) * m(3, 3) +
     346             :            m(0, 1) * m(1, 2) * m(2, 0) * m(3, 3) +
     347             :            m(0, 2) * m(1, 0) * m(2, 1) * m(3, 3) -
     348             :            m(0, 0) * m(1, 2) * m(2, 1) * m(3, 3) -
     349             :            m(0, 1) * m(1, 0) * m(2, 2) * m(3, 3) +
     350             :            m(0, 0) * m(1, 1) * m(2, 2) * m(3, 3);
     351             : }
     352             : 
     353             : template <typename T>
     354             : Matrix<1, 1, T> computeInverse(const Matrix<1, 1, T>& m_)
     355             : {
     356             :     return Matrix<1, 1, T>(std::vector<T>(T(1) / m_(0, 0), 1));
     357             : }
     358             : 
     359             : template <typename T>
     360           1 : Matrix<2, 2, T> computeInverse(const Matrix<2, 2, T>& m_)
     361             : {
     362           1 :     const T det = computeDeterminant(m_);
     363           1 :     if (std::abs(det) < std::numeric_limits<T>::epsilon())
     364             :         return Matrix<2, 2, T>(
     365           1 :             std::vector<T>(4, std::numeric_limits<T>::quiet_NaN()));
     366             : 
     367           0 :     Matrix<2, 2, T> inverse;
     368           0 :     m_.getAdjugate(inverse);
     369           0 :     const T detinv = 1 / det;
     370           0 :     inverse(0, 0) *= detinv;
     371           0 :     inverse(0, 1) *= detinv;
     372           0 :     inverse(1, 0) *= detinv;
     373           0 :     inverse(1, 1) *= detinv;
     374           0 :     return inverse;
     375             : }
     376             : 
     377             : template <typename T>
     378           1 : Matrix<3, 3, T> computeInverse(const Matrix<3, 3, T>& m_)
     379             : {
     380             :     // Invert a 3x3 using cofactors.  This is about 8 times faster than
     381             :     // the Numerical Recipes code which uses Gaussian elimination.
     382           1 :     Matrix<3, 3, T> inverse;
     383           1 :     inverse(0, 0) = m_(1, 1) * m_(2, 2) - m_(1, 2) * m_(2, 1);
     384           1 :     inverse(0, 1) = m_(0, 2) * m_(2, 1) - m_(0, 1) * m_(2, 2);
     385           1 :     inverse(0, 2) = m_(0, 1) * m_(1, 2) - m_(0, 2) * m_(1, 1);
     386           1 :     inverse(1, 0) = m_(1, 2) * m_(2, 0) - m_(1, 0) * m_(2, 2);
     387           1 :     inverse(1, 1) = m_(0, 0) * m_(2, 2) - m_(0, 2) * m_(2, 0);
     388           1 :     inverse(1, 2) = m_(0, 2) * m_(1, 0) - m_(0, 0) * m_(1, 2);
     389           1 :     inverse(2, 0) = m_(1, 0) * m_(2, 1) - m_(1, 1) * m_(2, 0);
     390           1 :     inverse(2, 1) = m_(0, 1) * m_(2, 0) - m_(0, 0) * m_(2, 1);
     391           1 :     inverse(2, 2) = m_(0, 0) * m_(1, 1) - m_(0, 1) * m_(1, 0);
     392             : 
     393           1 :     const T determinant = m_(0, 0) * inverse(0, 0) + m_(0, 1) * inverse(1, 0) +
     394           1 :                           m_(0, 2) * inverse(2, 0);
     395             : 
     396           1 :     if (std::abs(determinant) <= std::numeric_limits<T>::epsilon())
     397             :         return Matrix<3, 3, T>(
     398           1 :             std::vector<T>(9, std::numeric_limits<T>::quiet_NaN()));
     399             : 
     400           0 :     const T detinv = static_cast<T>(1.0) / determinant;
     401             : 
     402           0 :     inverse(0, 0) *= detinv;
     403           0 :     inverse(0, 1) *= detinv;
     404           0 :     inverse(0, 2) *= detinv;
     405           0 :     inverse(1, 0) *= detinv;
     406           0 :     inverse(1, 1) *= detinv;
     407           0 :     inverse(1, 2) *= detinv;
     408           0 :     inverse(2, 0) *= detinv;
     409           0 :     inverse(2, 1) *= detinv;
     410           0 :     inverse(2, 2) *= detinv;
     411           0 :     return inverse;
     412             : }
     413             : 
     414             : template <typename T>
     415           3 : Matrix<4, 4, T> computeInverse(const Matrix<4, 4, T>& m_)
     416             : {
     417           3 :     const T* array = m_.array;
     418             : 
     419             :     // tuned version from Claude Knaus
     420             :     /* first set of 2x2 determinants: 12 multiplications, 6 additions */
     421           3 :     const T t1[6] = {array[2] * array[7] - array[6] * array[3],
     422           3 :                      array[2] * array[11] - array[10] * array[3],
     423           3 :                      array[2] * array[15] - array[14] * array[3],
     424           3 :                      array[6] * array[11] - array[10] * array[7],
     425           3 :                      array[6] * array[15] - array[14] * array[7],
     426          15 :                      array[10] * array[15] - array[14] * array[11]};
     427             : 
     428             :     /* first half of comatrix: 24 multiplications, 16 additions */
     429           3 :     Matrix<4, 4, T> inv;
     430           3 :     inv.array[0] = array[5] * t1[5] - array[9] * t1[4] + array[13] * t1[3];
     431           3 :     inv.array[1] = array[9] * t1[2] - array[13] * t1[1] - array[1] * t1[5];
     432           3 :     inv.array[2] = array[13] * t1[0] - array[5] * t1[2] + array[1] * t1[4];
     433           3 :     inv.array[3] = array[5] * t1[1] - array[1] * t1[3] - array[9] * t1[0];
     434           3 :     inv.array[4] = array[8] * t1[4] - array[4] * t1[5] - array[12] * t1[3];
     435           3 :     inv.array[5] = array[0] * t1[5] - array[8] * t1[2] + array[12] * t1[1];
     436           3 :     inv.array[6] = array[4] * t1[2] - array[12] * t1[0] - array[0] * t1[4];
     437           3 :     inv.array[7] = array[0] * t1[3] - array[4] * t1[1] + array[8] * t1[0];
     438             : 
     439             :     /* second set of 2x2 determinants: 12 multiplications, 6 additions */
     440           3 :     const T t2[6] = {array[0] * array[5] - array[4] * array[1],
     441           3 :                      array[0] * array[9] - array[8] * array[1],
     442           3 :                      array[0] * array[13] - array[12] * array[1],
     443           3 :                      array[4] * array[9] - array[8] * array[5],
     444           3 :                      array[4] * array[13] - array[12] * array[5],
     445          15 :                      array[8] * array[13] - array[12] * array[9]};
     446             : 
     447             :     /* second half of comatrix: 24 multiplications, 16 additions */
     448           3 :     inv.array[8] = array[7] * t2[5] - array[11] * t2[4] + array[15] * t2[3];
     449           3 :     inv.array[9] = array[11] * t2[2] - array[15] * t2[1] - array[3] * t2[5];
     450           3 :     inv.array[10] = array[15] * t2[0] - array[7] * t2[2] + array[3] * t2[4];
     451           3 :     inv.array[11] = array[7] * t2[1] - array[3] * t2[3] - array[11] * t2[0];
     452           3 :     inv.array[12] = array[10] * t2[4] - array[6] * t2[5] - array[14] * t2[3];
     453           3 :     inv.array[13] = array[2] * t2[5] - array[10] * t2[2] + array[14] * t2[1];
     454           3 :     inv.array[14] = array[6] * t2[2] - array[14] * t2[0] - array[2] * t2[4];
     455           3 :     inv.array[15] = array[2] * t2[3] - array[6] * t2[1] + array[10] * t2[0];
     456             : 
     457             :     /* determinant: 4 multiplications, 3 additions */
     458           6 :     const T determinant = array[0] * inv.array[0] + array[4] * inv.array[1] +
     459           6 :                           array[8] * inv.array[2] + array[12] * inv.array[3];
     460             : 
     461             :     /* division: 16 multiplications, 1 division */
     462           3 :     const T detinv = T(1) / determinant;
     463          51 :     for (size_t i = 0; i != 16; ++i)
     464          48 :         inv.array[i] *= detinv;
     465           3 :     return inv;
     466             : }
     467             : 
     468             : template <size_t R, size_t C, typename T>
     469           0 : Matrix<R, C, T> computeInverse(const Matrix<R, C, T>&)
     470             : {
     471           0 :     throw std::runtime_error("Can't compute inverse of this matrix");
     472             : }
     473             : 
     474             : /** @return the transposed of a matrix */
     475             : template <size_t R, size_t C, typename T>
     476           1 : inline Matrix<C, R, T> transpose(const Matrix<R, C, T>& matrix)
     477             : {
     478           1 :     Matrix<C, R, T> transposed;
     479           4 :     for (size_t row = 0; row < R; ++row)
     480          12 :         for (size_t col = 0; col < C; ++col)
     481           9 :             transposed(col, row) = matrix(row, col);
     482           1 :     return transposed;
     483             : }
     484             : //@}
     485             : 
     486             : template <size_t R, size_t C, typename T>
     487          14 : Matrix<R, C, T>::Matrix()
     488          14 :     : array() // http://stackoverflow.com/questions/5602030
     489             : {
     490             :     if (R == C)
     491          64 :         for (size_t i = 0; i < R; ++i)
     492          50 :             (*this)(i, i) = 1;
     493          14 : }
     494             : 
     495             : template <size_t R, size_t C, typename T>
     496           2 : Matrix<R, C, T>::Matrix(const T* begin_, const T* end_)
     497           2 :     : array() // http://stackoverflow.com/questions/5602030
     498             : {
     499           2 :     const T* to = std::min(end_, begin_ + R * C);
     500           2 :     ::memcpy(array, begin_, (to - begin_) * sizeof(T));
     501           2 : }
     502             : 
     503             : template <size_t R, size_t C, typename T>
     504           2 : Matrix<R, C, T>::Matrix(const std::vector<T>& values)
     505             : {
     506           2 :     *this = values;
     507           2 : }
     508             : 
     509             : template <size_t R, size_t C, typename T>
     510             : template <size_t P, size_t Q>
     511           1 : Matrix<R, C, T>::Matrix(const Matrix<P, Q, T>& source)
     512             : {
     513           1 :     *this = source;
     514           1 : }
     515             : 
     516             : template <size_t R, size_t C, typename T>
     517             : template <size_t O>
     518           1 : Matrix<R, C, T>::Matrix(
     519             :     const Quaternion<T>& rotation, const vector<O, T>& translation,
     520             :     typename std::enable_if<R == O + 1 && C == O + 1 && O == 3>::type*)
     521             : {
     522           1 :     *this = rotation.getRotationMatrix();
     523           1 :     setTranslation(translation);
     524           1 :     (*this)(3, 0) = 0;
     525           1 :     (*this)(3, 1) = 0;
     526           1 :     (*this)(3, 2) = 0;
     527           1 :     (*this)(3, 3) = 1;
     528           1 : }
     529             : 
     530             : template <size_t R, size_t C, typename T>
     531             : template <size_t S>
     532           1 : Matrix<R, C, T>::Matrix(
     533             :     const vector<S, T>& eye, const vector<S, T>& lookat, const vector<S, T>& up,
     534             :     typename std::enable_if<R == S + 1 && C == S + 1 && S == 3>::type*)
     535           1 :     : array() // http://stackoverflow.com/questions/5602030
     536             : {
     537           1 :     const vector<3, T> f(vmml::normalize(lookat - eye));
     538           1 :     const vector<3, T> s(vmml::normalize(vmml::cross(f, up)));
     539           1 :     const vector<3, T> u(vmml::cross(s, f));
     540             : 
     541           1 :     (*this)(0, 0) = s.x();
     542           1 :     (*this)(0, 1) = s.y();
     543           1 :     (*this)(0, 2) = s.z();
     544           1 :     (*this)(1, 0) = u.x();
     545           1 :     (*this)(1, 1) = u.y();
     546           1 :     (*this)(1, 2) = u.z();
     547           1 :     (*this)(2, 0) = -f.x();
     548           1 :     (*this)(2, 1) = -f.y();
     549           1 :     (*this)(2, 2) = -f.z();
     550           1 :     (*this)(0, 3) = -vmml::dot(s, eye);
     551           1 :     (*this)(1, 3) = -vmml::dot(u, eye);
     552           1 :     (*this)(2, 3) = vmml::dot(f, eye);
     553           1 :     (*this)(3, 3) = 1;
     554           1 : }
     555             : 
     556             : template <size_t R, size_t C, typename T>
     557         180 : inline T& Matrix<R, C, T>::operator()(size_t rowIndex, size_t colIndex)
     558             : {
     559         180 :     if (rowIndex >= R || colIndex >= C)
     560           0 :         throw std::runtime_error("matrix( row, column ) index out of bounds");
     561         180 :     return array[colIndex * R + rowIndex];
     562             : }
     563             : 
     564             : template <size_t R, size_t C, typename T>
     565         141 : inline T Matrix<R, C, T>::operator()(size_t rowIndex, size_t colIndex) const
     566             : {
     567         141 :     if (rowIndex >= R || colIndex >= C)
     568           0 :         throw std::runtime_error("matrix( row, column ) index out of bounds");
     569         141 :     return array[colIndex * R + rowIndex];
     570             : }
     571             : 
     572             : template <size_t R, size_t C, typename T>
     573           1 : bool Matrix<R, C, T>::operator==(const Matrix<R, C, T>& other) const
     574             : {
     575          17 :     for (size_t i = 0; i < R * C; ++i)
     576          16 :         if (array[i] != other.array[i])
     577           0 :             return false;
     578           1 :     return true;
     579             : }
     580             : 
     581             : template <size_t R, size_t C, typename T>
     582           0 : bool Matrix<R, C, T>::operator!=(const Matrix<R, C, T>& other) const
     583             : {
     584           0 :     return !operator==(other);
     585             : }
     586             : 
     587             : template <size_t R, size_t C, typename T>
     588           0 : bool Matrix<R, C, T>::equals(const Matrix<R, C, T>& other,
     589             :                              const T tolerance) const
     590             : {
     591           0 :     for (size_t i = 0; i < R * C; ++i)
     592           0 :         if (std::abs(array[i] - other.array[i]) > tolerance)
     593           0 :             return false;
     594           0 :     return true;
     595             : }
     596             : 
     597             : template <size_t R, size_t C, typename T>
     598           1 : const Matrix<R, C, T>& Matrix<R, C, T>::operator=(const Matrix<R, C, T>& source)
     599             : {
     600           1 :     ::memcpy(array, source.array, R * C * sizeof(T));
     601           1 :     return *this;
     602             : }
     603             : 
     604             : template <size_t R, size_t C, typename T>
     605             : template <size_t P, size_t Q>
     606           2 : const Matrix<R, C, T>& Matrix<R, C, T>::operator=(const Matrix<P, Q, T>& source)
     607             : {
     608           2 :     const size_t minL = std::min(P, R);
     609           2 :     const size_t minC = std::min(Q, C);
     610             : 
     611           8 :     for (size_t i = 0; i < minL; ++i)
     612          24 :         for (size_t j = 0; j < minC; ++j)
     613          18 :             (*this)(i, j) = source(i, j);
     614           3 :     for (size_t i = minL; i < R; ++i)
     615           2 :         for (size_t j = minC; j < C; ++j)
     616           1 :             (*this)(i, j) = 0;
     617           2 :     return *this;
     618             : }
     619             : 
     620             : template <size_t R, size_t C, typename T>
     621           2 : void Matrix<R, C, T>::operator=(const std::vector<T>& values)
     622             : {
     623           2 :     const size_t to = std::min(R * C, values.size());
     624           2 :     ::memcpy(array, values.data(), to * sizeof(T));
     625           2 :     if (to < R * C)
     626             :     {
     627             : #ifdef _WIN32
     628             :         ::memset(array + to, 0, (R * C - to) * sizeof(T));
     629             : #else
     630           0 :         ::bzero(array + to, (R * C - to) * sizeof(T));
     631             : #endif
     632             :     }
     633           2 : }
     634             : 
     635             : template <size_t R, size_t C, typename T>
     636             : template <size_t P>
     637             : Matrix<R, C, T>& Matrix<R, C, T>::multiply(const Matrix<R, P, T>& left,
     638             :                                            const Matrix<P, C, T>& right)
     639             : {
     640             :     // Create copy for multiplication with self
     641             :     if (&left == this)
     642             :         return multiply(Matrix<R, P, T>(left), right);
     643             :     if (&right == this)
     644             :         return multiply(left, Matrix<R, P, T>(right));
     645             : 
     646             :     for (size_t rowIndex = 0; rowIndex < R; ++rowIndex)
     647             :     {
     648             :         for (size_t colIndex = 0; colIndex < C; ++colIndex)
     649             :         {
     650             :             T& component = (*this)(rowIndex, colIndex);
     651             :             component = static_cast<T>(0.0);
     652             :             for (size_t p = 0; p < P; p++)
     653             :                 component += left(rowIndex, p) * right(p, colIndex);
     654             :         }
     655             :     }
     656             :     return *this;
     657             : }
     658             : 
     659             : template <size_t R, size_t C, typename T>
     660             : template <size_t P>
     661             : Matrix<R, P, T> Matrix<R, C, T>::operator*(const Matrix<C, P, T>& other) const
     662             : {
     663             :     Matrix<R, P, T> result;
     664             :     return result.multiply(*this, other);
     665             : }
     666             : 
     667             : template <size_t R, size_t C, typename T>
     668             : template <size_t O, size_t P, typename>
     669             : Matrix<R, C, T>& Matrix<R, C, T>::operator*=(const Matrix<O, P, T>& right)
     670             : {
     671             :     Matrix<R, C, T> copy(*this);
     672             :     multiply(copy, right);
     673             :     return *this;
     674             : }
     675             : 
     676             : template <size_t R, size_t C, typename T>
     677           3 : vector<R, T> Matrix<R, C, T>::operator*(const vector<C, T>& vec) const
     678             : {
     679           3 :     vector<R, T> result;
     680             : 
     681             :     // this < R, 1 > = < R, P > * < P, 1 >
     682          13 :     for (size_t i = 0; i < R; ++i)
     683             :     {
     684          10 :         T tmp(0);
     685          44 :         for (size_t j = 0; j < C; ++j)
     686          34 :             tmp += (*this)(i, j) * vec(j);
     687          10 :         result(i) = tmp;
     688             :     }
     689           3 :     return result;
     690             : }
     691             : 
     692             : template <size_t R, size_t C, typename T>
     693           0 : inline Matrix<R, C, T> Matrix<R, C, T>::operator-() const
     694             : {
     695           0 :     Matrix<R, C, T> result;
     696           0 :     for (size_t i = 0; i < R * C; ++i)
     697           0 :         result.array[i] = -array[i];
     698           0 :     return result;
     699             : }
     700             : 
     701             : template <size_t R, size_t C, typename T>
     702           0 : vector<R, T> Matrix<R, C, T>::getColumn(const size_t index) const
     703             : {
     704           0 :     if (index >= C)
     705           0 :         throw std::runtime_error("getColumn() - index out of bounds.");
     706           0 :     vector<R, T> column;
     707           0 :     ::memcpy(&column.array[0], &array[R * index], R * sizeof(T));
     708           0 :     return column;
     709             : }
     710             : 
     711             : template <size_t R, size_t C, typename T>
     712           0 : void Matrix<R, C, T>::setColumn(size_t index, const vector<R, T>& column)
     713             : {
     714           0 :     if (index >= C)
     715           0 :         throw std::runtime_error("setColumn() - index out of bounds.");
     716           0 :     memcpy(array + R * index, column.array, R * sizeof(T));
     717           0 : }
     718             : 
     719             : template <size_t R, size_t C, typename T>
     720           4 : vector<C, T> Matrix<R, C, T>::getRow(size_t index) const
     721             : {
     722           4 :     if (index >= R)
     723           0 :         throw std::runtime_error("getRow() - index out of bounds.");
     724             : 
     725           4 :     vector<C, T> row;
     726          20 :     for (size_t colIndex = 0; colIndex < C; ++colIndex)
     727          16 :         row(colIndex) = (*this)(index, colIndex);
     728           4 :     return row;
     729             : }
     730             : 
     731             : template <size_t R, size_t C, typename T>
     732           0 : void Matrix<R, C, T>::setRow(size_t rowIndex, const vector<C, T>& row)
     733             : {
     734           0 :     if (rowIndex >= R)
     735           0 :         throw std::runtime_error("setRow() - index out of bounds.");
     736             : 
     737           0 :     for (size_t colIndex = 0; colIndex < C; ++colIndex)
     738           0 :         (*this)(rowIndex, colIndex) = row(colIndex);
     739           0 : }
     740             : 
     741             : template <size_t R, size_t C, typename T>
     742           0 : inline Matrix<R, C, T> Matrix<R, C, T>::operator+(
     743             :     const Matrix<R, C, T>& other) const
     744             : {
     745           0 :     Matrix<R, C, T> result(*this);
     746           0 :     result += other;
     747           0 :     return result;
     748             : }
     749             : 
     750             : template <size_t R, size_t C, typename T>
     751           0 : void Matrix<R, C, T>::operator+=(const Matrix<R, C, T>& other)
     752             : {
     753           0 :     for (size_t i = 0; i < R * C; ++i)
     754           0 :         array[i] += other.array[i];
     755           0 : }
     756             : 
     757             : template <size_t R, size_t C, typename T>
     758           0 : inline Matrix<R, C, T> Matrix<R, C, T>::operator-(
     759             :     const Matrix<R, C, T>& other) const
     760             : {
     761           0 :     Matrix<R, C, T> result(*this);
     762           0 :     result -= other;
     763           0 :     return result;
     764             : }
     765             : 
     766             : template <size_t R, size_t C, typename T>
     767           0 : void Matrix<R, C, T>::operator-=(const Matrix<R, C, T>& other)
     768             : {
     769           0 :     for (size_t i = 0; i < R * C; ++i)
     770           0 :         array[i] -= other.array[i];
     771           0 : }
     772             : 
     773             : template <size_t R, size_t C, typename T>
     774             : template <size_t O, size_t P>
     775             : Matrix<O, P, T> Matrix<R, C, T>::getSubMatrix(
     776             :     size_t rowOffset, size_t colOffset,
     777             :     typename std::enable_if<O <= R && P <= C>::type*) const
     778             : {
     779             :     Matrix<O, P, T> result;
     780             :     if (O + rowOffset > R || P + colOffset > C)
     781             :         throw std::runtime_error("index out of bounds.");
     782             : 
     783             :     for (size_t row = 0; row < O; ++row)
     784             :         for (size_t col = 0; col < P; ++col)
     785             :             result(row, col) = (*this)(rowOffset + row, colOffset + col);
     786             : 
     787             :     return result;
     788             : }
     789             : 
     790             : template <size_t R, size_t C, typename T>
     791             : template <size_t O, size_t P>
     792             : typename std::enable_if<O <= R && P <= C>::type* Matrix<R, C, T>::setSubMatrix(
     793             :     const Matrix<O, P, T>& sub_matrix, size_t rowOffset, size_t colOffset)
     794             : {
     795             :     for (size_t row = 0; row < O; ++row)
     796             :         for (size_t col = 0; col < P; ++col)
     797             :             (*this)(rowOffset + row, colOffset + col) = sub_matrix(row, col);
     798             :     return 0; // for sfinae
     799             : }
     800             : 
     801             : template <size_t R, size_t C, typename T>
     802           5 : Matrix<R, C, T> Matrix<R, C, T>::inverse() const
     803             : {
     804           5 :     return computeInverse(*this);
     805             : }
     806             : 
     807             : template <size_t R, size_t C, typename T>
     808             : template <size_t O, size_t P>
     809             : typename std::enable_if<O == P && R == C && O == R && R >= 2>::type*
     810           0 :     Matrix<R, C, T>::getAdjugate(Matrix<O, P, T>& adjugate) const
     811             : {
     812           0 :     getCofactors(adjugate);
     813           0 :     adjugate = transpose(adjugate);
     814           0 :     return 0;
     815             : }
     816             : 
     817             : template <size_t R, size_t C, typename T>
     818             : template <size_t O, size_t P>
     819             : typename std::enable_if<O == P && R == C && O == R && R >= 2>::type*
     820           0 :     Matrix<R, C, T>::getCofactors(Matrix<O, P, T>& cofactors) const
     821             : {
     822           0 :     Matrix<R - 1, C - 1, T> minor_;
     823             : 
     824           0 :     const size_t _negate = 1u;
     825           0 :     for (size_t rowIndex = 0; rowIndex < R; ++rowIndex)
     826           0 :         for (size_t colIndex = 0; colIndex < C; ++colIndex)
     827           0 :             if ((rowIndex + colIndex) & _negate)
     828           0 :                 cofactors(rowIndex, colIndex) =
     829           0 :                     -getMinor(minor_, rowIndex, colIndex);
     830             :             else
     831           0 :                 cofactors(rowIndex, colIndex) =
     832             :                     getMinor(minor_, rowIndex, colIndex);
     833             : 
     834           0 :     return 0;
     835             : }
     836             : 
     837             : template <size_t R, size_t C, typename T>
     838             : template <size_t O, size_t P>
     839           0 : T Matrix<R, C, T>::getMinor(
     840             :     Matrix<O, P, T>& minor_, size_t row_to_cut, size_t col_to_cut,
     841             :     typename std::enable_if<O == R - 1 && P == C - 1 && R == C &&
     842             :                             R >= 2>::type*) const
     843             : {
     844           0 :     size_t rowOffset = 0;
     845           0 :     size_t colOffset = 0;
     846           0 :     for (size_t rowIndex = 0; rowIndex < R; ++rowIndex)
     847             :     {
     848           0 :         if (rowIndex == row_to_cut)
     849           0 :             rowOffset = -1;
     850             :         else
     851             :         {
     852           0 :             for (size_t colIndex = 0; colIndex < R; ++colIndex)
     853             :             {
     854           0 :                 if (colIndex == col_to_cut)
     855           0 :                     colOffset = -1;
     856             :                 else
     857           0 :                     minor_(rowIndex + rowOffset, colIndex + colOffset) =
     858           0 :                         (*this)(rowIndex, colIndex);
     859             :             }
     860           0 :             colOffset = 0;
     861             :         }
     862             :     }
     863           0 :     return computeDeterminant(minor_);
     864             : }
     865             : 
     866             : template <size_t R, size_t C, typename T>
     867             : template <typename TT>
     868             : Matrix<R, C, T>& Matrix<R, C, T>::rotate_x(
     869             :     const TT angle_, typename std::enable_if<R == C && R == 4, TT>::type*)
     870             : {
     871             :     const T angle = static_cast<T>(angle_);
     872             :     const T sine = std::sin(angle);
     873             :     const T cosine = std::cos(angle);
     874             : 
     875             :     T tmp;
     876             : 
     877             :     tmp = array[4] * cosine + array[8] * sine;
     878             :     array[8] = -array[4] * sine + array[8] * cosine;
     879             :     array[4] = tmp;
     880             : 
     881             :     tmp = array[5] * cosine + array[9] * sine;
     882             :     array[9] = -array[5] * sine + array[9] * cosine;
     883             :     array[5] = tmp;
     884             : 
     885             :     tmp = array[6] * cosine + array[10] * sine;
     886             :     array[10] = -array[6] * sine + array[10] * cosine;
     887             :     array[6] = tmp;
     888             : 
     889             :     tmp = array[7] * cosine + array[11] * sine;
     890             :     array[11] = -array[7] * sine + array[11] * cosine;
     891             :     array[7] = tmp;
     892             : 
     893             :     return *this;
     894             : }
     895             : 
     896             : template <size_t R, size_t C, typename T>
     897             : template <typename TT>
     898             : Matrix<R, C, T>& Matrix<R, C, T>::rotate_y(
     899             :     const TT angle_, typename std::enable_if<R == C && R == 4, TT>::type*)
     900             : {
     901             :     const T angle = static_cast<T>(angle_);
     902             :     const T sine = std::sin(angle);
     903             :     const T cosine = std::cos(angle);
     904             : 
     905             :     T tmp;
     906             : 
     907             :     tmp = array[0] * cosine - array[8] * sine;
     908             :     array[8] = array[0] * sine + array[8] * cosine;
     909             :     array[0] = tmp;
     910             : 
     911             :     tmp = array[1] * cosine - array[9] * sine;
     912             :     array[9] = array[1] * sine + array[9] * cosine;
     913             :     array[1] = tmp;
     914             : 
     915             :     tmp = array[2] * cosine - array[10] * sine;
     916             :     array[10] = array[2] * sine + array[10] * cosine;
     917             :     array[2] = tmp;
     918             : 
     919             :     tmp = array[3] * cosine - array[11] * sine;
     920             :     array[11] = array[3] * sine + array[11] * cosine;
     921             :     array[3] = tmp;
     922             : 
     923             :     return *this;
     924             : }
     925             : 
     926             : template <size_t R, size_t C, typename T>
     927             : template <typename TT>
     928             : Matrix<R, C, T>& Matrix<R, C, T>::rotate_z(
     929             :     const TT angle_, typename std::enable_if<R == C && R == 4, TT>::type*)
     930             : {
     931             :     const T angle = static_cast<T>(angle_);
     932             :     const T sine = std::sin(angle);
     933             :     const T cosine = std::cos(angle);
     934             : 
     935             :     T tmp;
     936             : 
     937             :     tmp = array[0] * cosine + array[4] * sine;
     938             :     array[4] = -array[0] * sine + array[4] * cosine;
     939             :     array[0] = tmp;
     940             : 
     941             :     tmp = array[1] * cosine + array[5] * sine;
     942             :     array[5] = -array[1] * sine + array[5] * cosine;
     943             :     array[1] = tmp;
     944             : 
     945             :     tmp = array[2] * cosine + array[6] * sine;
     946             :     array[6] = -array[2] * sine + array[6] * cosine;
     947             :     array[2] = tmp;
     948             : 
     949             :     tmp = array[3] * cosine + array[7] * sine;
     950             :     array[7] = -array[3] * sine + array[7] * cosine;
     951             :     array[3] = tmp;
     952             : 
     953             :     return *this;
     954             : }
     955             : 
     956             : template <size_t R, size_t C, typename T>
     957             : template <typename TT>
     958             : Matrix<R, C, T>& Matrix<R, C, T>::pre_rotate_x(
     959             :     const TT angle_, typename std::enable_if<R == C && R == 4, TT>::type*)
     960             : {
     961             :     const T angle = static_cast<T>(angle_);
     962             :     const T sine = std::sin(angle);
     963             :     const T cosine = std::cos(angle);
     964             : 
     965             :     T tmp;
     966             : 
     967             :     tmp = array[1];
     968             :     array[1] = array[1] * cosine + array[2] * sine;
     969             :     array[2] = tmp * -sine + array[2] * cosine;
     970             : 
     971             :     tmp = array[5];
     972             :     array[5] = array[5] * cosine + array[6] * sine;
     973             :     array[6] = tmp * -sine + array[6] * cosine;
     974             : 
     975             :     tmp = array[9];
     976             :     array[9] = array[9] * cosine + array[10] * sine;
     977             :     array[10] = tmp * -sine + array[10] * cosine;
     978             : 
     979             :     tmp = array[13];
     980             :     array[13] = array[13] * cosine + array[14] * sine;
     981             :     array[14] = tmp * -sine + array[14] * cosine;
     982             : 
     983             :     return *this;
     984             : }
     985             : 
     986             : template <size_t R, size_t C, typename T>
     987             : template <typename TT>
     988             : Matrix<R, C, T>& Matrix<R, C, T>::pre_rotate_y(
     989             :     const TT angle_, typename std::enable_if<R == C && R == 4, TT>::type*)
     990             : {
     991             :     const T angle = static_cast<T>(angle_);
     992             :     const T sine = std::sin(angle);
     993             :     const T cosine = std::cos(angle);
     994             : 
     995             :     T tmp;
     996             : 
     997             :     tmp = array[0];
     998             :     array[0] = array[0] * cosine - array[2] * sine;
     999             :     array[2] = tmp * sine + array[2] * cosine;
    1000             : 
    1001             :     tmp = array[4];
    1002             :     array[4] = array[4] * cosine - array[6] * sine;
    1003             :     array[6] = tmp * sine + array[6] * cosine;
    1004             : 
    1005             :     tmp = array[8];
    1006             :     array[8] = array[8] * cosine - array[10] * sine;
    1007             :     array[10] = tmp * sine + array[10] * cosine;
    1008             : 
    1009             :     tmp = array[12];
    1010             :     array[12] = array[12] * cosine - array[14] * sine;
    1011             :     array[14] = tmp * sine + array[14] * cosine;
    1012             : 
    1013             :     return *this;
    1014             : }
    1015             : 
    1016             : template <size_t R, size_t C, typename T>
    1017             : template <typename TT>
    1018             : Matrix<R, C, T>& Matrix<R, C, T>::pre_rotate_z(
    1019             :     const TT angle_, typename std::enable_if<R == C && R == 4, TT>::type*)
    1020             : {
    1021             :     const T angle = static_cast<T>(angle_);
    1022             :     const T sine = std::sin(angle);
    1023             :     const T cosine = std::cos(angle);
    1024             : 
    1025             :     T tmp;
    1026             : 
    1027             :     tmp = array[0];
    1028             :     array[0] = array[0] * cosine + array[1] * sine;
    1029             :     array[1] = tmp * -sine + array[1] * cosine;
    1030             : 
    1031             :     tmp = array[4];
    1032             :     array[4] = array[4] * cosine + array[5] * sine;
    1033             :     array[5] = tmp * -sine + array[5] * cosine;
    1034             : 
    1035             :     tmp = array[8];
    1036             :     array[8] = array[8] * cosine + array[9] * sine;
    1037             :     array[9] = tmp * -sine + array[9] * cosine;
    1038             : 
    1039             :     tmp = array[12];
    1040             :     array[12] = array[12] * cosine + array[13] * sine;
    1041             :     array[13] = tmp * -sine + array[13] * cosine;
    1042             : 
    1043             :     return *this;
    1044             : }
    1045             : 
    1046             : template <size_t R, size_t C, typename T>
    1047             : template <typename TT>
    1048             : inline Matrix<R, C, T>& Matrix<R, C, T>::scale(
    1049             :     const vector<3, TT>& scale_,
    1050             :     typename std::enable_if<R == C && R == 4, TT>::type*)
    1051             : {
    1052             :     array[0] *= scale_[0];
    1053             :     array[1] *= scale_[0];
    1054             :     array[2] *= scale_[0];
    1055             :     array[3] *= scale_[0];
    1056             :     array[4] *= scale_[1];
    1057             :     array[5] *= scale_[1];
    1058             :     array[6] *= scale_[1];
    1059             :     array[7] *= scale_[1];
    1060             :     array[8] *= scale_[2];
    1061             :     array[9] *= scale_[2];
    1062             :     array[10] *= scale_[2];
    1063             :     array[11] *= scale_[2];
    1064             :     return *this;
    1065             : }
    1066             : 
    1067             : template <size_t R, size_t C, typename T>
    1068             : template <typename TT>
    1069             : inline Matrix<R, C, T>& Matrix<R, C, T>::scaleTranslation(
    1070             :     const vector<3, TT>& scale_,
    1071             :     typename std::enable_if<R == C && R == 4, TT>::type*)
    1072             : {
    1073             :     array[12] *= static_cast<T>(scale_[0]);
    1074             :     array[13] *= static_cast<T>(scale_[1]);
    1075             :     array[14] *= static_cast<T>(scale_[2]);
    1076             :     return *this;
    1077             : }
    1078             : 
    1079             : template <size_t R, size_t C, typename T>
    1080           1 : inline Matrix<R, C, T>& Matrix<R, C, T>::setTranslation(
    1081             :     const vector<C - 1, T>& trans)
    1082             : {
    1083           4 :     for (size_t i = 0; i < C - 1; ++i)
    1084           3 :         array[i + R * (C - 1)] = trans[i];
    1085           1 :     return *this;
    1086             : }
    1087             : 
    1088             : template <size_t R, size_t C, typename T>
    1089           0 : inline vector<C - 1, T> Matrix<R, C, T>::getTranslation() const
    1090             : {
    1091           0 :     vector<C - 1, T> result;
    1092           0 :     for (size_t i = 0; i < C - 1; ++i)
    1093           0 :         result[i] = array[i + R * (C - 1)];
    1094           0 :     return result;
    1095             : }
    1096             : 
    1097             : template <size_t R, size_t C, typename T>
    1098             : template <size_t S>
    1099           1 : void Matrix<R, C, T>::getLookAt(
    1100             :     vector<S, T>& eye, vector<S, T>& lookAt, vector<S, T>& up,
    1101             :     typename std::enable_if<R == S + 1 && C == S + 1 && S == 3>::type*) const
    1102             : {
    1103           1 :     const Matrix<4, 4, T>& inv = inverse();
    1104           1 :     const Matrix<3, 3, T> rotation(transpose(Matrix<3, 3, T>(*this)));
    1105             : 
    1106           1 :     eye = vector<S, T>(inv * vector<4, T>::zero());
    1107           1 :     up = rotation * vector<3, T>::up();
    1108           1 :     lookAt = rotation * vector<3, T>::forward();
    1109           1 :     lookAt.normalize();
    1110           1 :     lookAt = eye + lookAt;
    1111           1 : }
    1112             : 
    1113             : } // namespace vmml
    1114             : 
    1115             : #endif

Generated by: LCOV version 1.11