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