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 0 : os << std::setw(10) << matrix( rowIndex, colIndex ) << " ";
285 0 : os << "|" << std::endl;
286 : }
287 0 : os.precision( prec );
288 0 : os.setf( flags );
289 0 : return os;
290 : };
291 :
292 : T array[ R * C ]; //!< column by column storage
293 : };
294 : }
295 :
296 : #include <vmmlib/quaternion.hpp>
297 : #include <vmmlib/vector.hpp>
298 :
299 : namespace vmml
300 : {
301 : /** @name Free matrix functions */
302 : //@{
303 : template< typename T >
304 0 : inline T computeDeterminant( const Matrix< 1, 1, T >& matrix_ )
305 : {
306 0 : return matrix_.array[ 0 ];
307 : }
308 :
309 : template< typename T >
310 1 : inline T computeDeterminant( const Matrix< 2, 2, T >& matrix_ )
311 : {
312 1 : return matrix_( 0, 0 ) * matrix_( 1, 1 ) - matrix_( 0, 1 ) * matrix_( 1, 0);
313 : }
314 :
315 : template< typename T >
316 : inline T computeDeterminant( const Matrix< 3, 3, T >& m_ )
317 : {
318 : return m_( 0,0 ) * ( m_( 1,1 ) * m_( 2,2 ) - m_( 1,2 ) * m_( 2,1 )) +
319 : m_( 0,1 ) * ( m_( 1,2 ) * m_( 2,0 ) - m_( 1,0 ) * m_( 2,2 )) +
320 : m_( 0,2 ) * ( m_( 1,0 ) * m_( 2,1 ) - m_( 1,1 ) * m_( 2,0 ));
321 : }
322 :
323 : template< typename T >
324 : inline T computeDeterminant( const Matrix< 4, 4, T >& m )
325 : {
326 : return m( 0, 3 ) * m( 1, 2 ) * m( 2, 1 ) * m( 3, 0 )
327 : - m( 0, 2 ) * m( 1, 3 ) * m( 2, 1 ) * m( 3, 0 )
328 : - m( 0, 3 ) * m( 1, 1 ) * m( 2, 2 ) * m( 3, 0 )
329 : + m( 0, 1 ) * m( 1, 3 ) * m( 2, 2 ) * m( 3, 0 )
330 : + m( 0, 2 ) * m( 1, 1 ) * m( 2, 3 ) * m( 3, 0 )
331 : - m( 0, 1 ) * m( 1, 2 ) * m( 2, 3 ) * m( 3, 0 )
332 : - m( 0, 3 ) * m( 1, 2 ) * m( 2, 0 ) * m( 3, 1 )
333 : + m( 0, 2 ) * m( 1, 3 ) * m( 2, 0 ) * m( 3, 1 )
334 : + m( 0, 3 ) * m( 1, 0 ) * m( 2, 2 ) * m( 3, 1 )
335 : - m( 0, 0 ) * m( 1, 3 ) * m( 2, 2 ) * m( 3, 1 )
336 : - m( 0, 2 ) * m( 1, 0 ) * m( 2, 3 ) * m( 3, 1 )
337 : + m( 0, 0 ) * m( 1, 2 ) * m( 2, 3 ) * m( 3, 1 )
338 : + m( 0, 3 ) * m( 1, 1 ) * m( 2, 0 ) * m( 3, 2 )
339 : - m( 0, 1 ) * m( 1, 3 ) * m( 2, 0 ) * m( 3, 2 )
340 : - m( 0, 3 ) * m( 1, 0 ) * m( 2, 1 ) * m( 3, 2 )
341 : + m( 0, 0 ) * m( 1, 3 ) * m( 2, 1 ) * m( 3, 2 )
342 : + m( 0, 1 ) * m( 1, 0 ) * m( 2, 3 ) * m( 3, 2 )
343 : - m( 0, 0 ) * m( 1, 1 ) * m( 2, 3 ) * m( 3, 2 )
344 : - m( 0, 2 ) * m( 1, 1 ) * m( 2, 0 ) * m( 3, 3 )
345 : + m( 0, 1 ) * m( 1, 2 ) * m( 2, 0 ) * m( 3, 3 )
346 : + m( 0, 2 ) * m( 1, 0 ) * m( 2, 1 ) * m( 3, 3 )
347 : - m( 0, 0 ) * m( 1, 2 ) * m( 2, 1 ) * m( 3, 3 )
348 : - m( 0, 1 ) * m( 1, 0 ) * m( 2, 2 ) * m( 3, 3 )
349 : + m( 0, 0 ) * m( 1, 1 ) * m( 2, 2 ) * m( 3, 3 );
350 : }
351 :
352 : template< typename T >
353 : Matrix< 1, 1, T > computeInverse( const Matrix< 1, 1, T >& m_ )
354 : {
355 : return Matrix< 1, 1, T >( std::vector< T >( T(1) / m_( 0, 0 ), 1 ));
356 : }
357 :
358 : template< typename T >
359 1 : Matrix< 2, 2, T > computeInverse( const Matrix< 2, 2, T >& m_ )
360 : {
361 1 : const T det = computeDeterminant( m_ );
362 1 : if( std::abs( det ) < std::numeric_limits< T >::epsilon( ))
363 : return Matrix< 2, 2, T >(
364 1 : std::vector< T >( 4, std::numeric_limits< T >::quiet_NaN( )));
365 :
366 0 : Matrix< 2, 2, T > inverse;
367 0 : m_.getAdjugate( inverse );
368 0 : const T detinv = 1 / det;
369 0 : inverse( 0, 0 ) *= detinv;
370 0 : inverse( 0, 1 ) *= detinv;
371 0 : inverse( 1, 0 ) *= detinv;
372 0 : inverse( 1, 1 ) *= detinv;
373 0 : return inverse;
374 : }
375 :
376 : template< typename T >
377 1 : Matrix< 3, 3, T > computeInverse( const Matrix< 3, 3, T >& m_ )
378 : {
379 : // Invert a 3x3 using cofactors. This is about 8 times faster than
380 : // the Numerical Recipes code which uses Gaussian elimination.
381 1 : Matrix< 3, 3, T > inverse;
382 1 : inverse( 0, 0 ) = m_( 1, 1 ) * m_( 2, 2 ) - m_( 1, 2 ) * m_( 2, 1 );
383 1 : inverse( 0, 1 ) = m_( 0, 2 ) * m_( 2, 1 ) - m_( 0, 1 ) * m_( 2, 2 );
384 1 : inverse( 0, 2 ) = m_( 0, 1 ) * m_( 1, 2 ) - m_( 0, 2 ) * m_( 1, 1 );
385 1 : inverse( 1, 0 ) = m_( 1, 2 ) * m_( 2, 0 ) - m_( 1, 0 ) * m_( 2, 2 );
386 1 : inverse( 1, 1 ) = m_( 0, 0 ) * m_( 2, 2 ) - m_( 0, 2 ) * m_( 2, 0 );
387 1 : inverse( 1, 2 ) = m_( 0, 2 ) * m_( 1, 0 ) - m_( 0, 0 ) * m_( 1, 2 );
388 1 : inverse( 2, 0 ) = m_( 1, 0 ) * m_( 2, 1 ) - m_( 1, 1 ) * m_( 2, 0 );
389 1 : inverse( 2, 1 ) = m_( 0, 1 ) * m_( 2, 0 ) - m_( 0, 0 ) * m_( 2, 1 );
390 1 : inverse( 2, 2 ) = m_( 0, 0 ) * m_( 1, 1 ) - m_( 0, 1 ) * m_( 1, 0 );
391 :
392 2 : const T determinant = m_( 0, 0 ) * inverse( 0, 0 ) +
393 1 : 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 > inline
476 1 : 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 > template< size_t O >
517 1 : Matrix< R, C, T >::Matrix( const Quaternion< T >& rotation,
518 : const vector< O, T >& translation,
519 : typename enable_if< R == O+1 && C == O+1 && O == 3 >::type* )
520 : {
521 1 : *this = rotation.getRotationMatrix();
522 1 : setTranslation( translation );
523 1 : (*this)( 3, 0 ) = 0;
524 1 : (*this)( 3, 1 ) = 0;
525 1 : (*this)( 3, 2 ) = 0;
526 1 : (*this)( 3, 3 ) = 1;
527 1 : }
528 :
529 : template< size_t R, size_t C, typename T > template< size_t S >
530 1 : Matrix< R, C, T >::Matrix( const vector< S, T >& eye,
531 : const vector< S, T >& lookat,
532 : const vector< S, T >& up,
533 : typename enable_if< R == S+1 && C == S+1 && S == 3 >::type* )
534 1 : : array() // http://stackoverflow.com/questions/5602030
535 : {
536 1 : const vector< 3, T > f( vmml::normalize( lookat - eye ));
537 1 : const vector< 3, T > s( vmml::normalize( vmml::cross( f, up )));
538 1 : const vector< 3, T > u( vmml::cross( s, f ));
539 :
540 1 : (*this)( 0, 0 ) = s.x();
541 1 : (*this)( 0, 1 ) = s.y();
542 1 : (*this)( 0, 2 ) = s.z();
543 1 : (*this)( 1, 0 ) = u.x();
544 1 : (*this)( 1, 1 ) = u.y();
545 1 : (*this)( 1, 2 ) = u.z();
546 1 : (*this)( 2, 0 ) = -f.x();
547 1 : (*this)( 2, 1 ) = -f.y();
548 1 : (*this)( 2, 2 ) = -f.z();
549 1 : (*this)( 0, 3 ) = -vmml::dot( s, eye );
550 1 : (*this)( 1, 3 ) = -vmml::dot( u, eye );
551 1 : (*this)( 2, 3 ) = vmml::dot( f, eye );
552 1 : (*this)( 3, 3 ) = 1;
553 1 : }
554 :
555 : template< size_t R, size_t C, typename T > inline
556 180 : T& Matrix< R, C, T >::operator()( size_t rowIndex, size_t colIndex )
557 : {
558 180 : if ( rowIndex >= R || colIndex >= C )
559 0 : throw std::runtime_error( "matrix( row, column ) index out of bounds" );
560 180 : return array[ colIndex * R + rowIndex ];
561 : }
562 :
563 : template< size_t R, size_t C, typename T > inline
564 141 : T Matrix< R, C, T >::operator()( size_t rowIndex, size_t colIndex ) const
565 : {
566 141 : if ( rowIndex >= R || colIndex >= C )
567 0 : throw std::runtime_error( "matrix( row, column ) index out of bounds" );
568 141 : return array[ colIndex * R + rowIndex ];
569 : }
570 :
571 : template< size_t R, size_t C, typename T >
572 1 : bool Matrix< R, C, T >::operator==( const Matrix< R, C, T >& other ) const
573 : {
574 17 : for( size_t i = 0; i < R * C; ++i )
575 16 : if( array[ i ] != other.array[ i ])
576 0 : return false;
577 1 : return true;
578 : }
579 :
580 : template< size_t R, size_t C, typename T >
581 0 : bool Matrix< R, C, T >::operator!=( const Matrix< R, C, T >& other ) const
582 : {
583 0 : return ! operator==( other );
584 : }
585 :
586 : template< size_t R, size_t C, typename T >
587 0 : bool Matrix< R, C, T >::equals( const Matrix< R, C, T >& other,
588 : const T tolerance ) const
589 : {
590 0 : for( size_t i = 0; i < R * C; ++i )
591 0 : if( std::abs( array[ i ] - other.array[ i ]) > tolerance )
592 0 : return false;
593 0 : return true;
594 : }
595 :
596 : template< size_t R, size_t C, typename T > const Matrix< R, C, T >&
597 1 : Matrix< R, C, T >::operator=( const Matrix< R, C, T >& source )
598 : {
599 1 : ::memcpy( array, source.array, R * C * sizeof( T ));
600 1 : return *this;
601 : }
602 :
603 : template< size_t R, size_t C, typename T > template< size_t P, size_t Q >
604 : const Matrix< R, C, T >&
605 2 : Matrix< R, C, T >::operator=( const Matrix< P, Q, T >& source )
606 : {
607 2 : const size_t minL = std::min( P, R );
608 2 : const size_t minC = std::min( Q, C );
609 :
610 8 : for ( size_t i = 0 ; i < minL ; ++i )
611 24 : for ( size_t j = 0 ; j < minC ; ++j )
612 18 : (*this)( i, j ) = source( i, j );
613 3 : for ( size_t i = minL ; i< R ; ++i )
614 2 : for ( size_t j = minC ; j < C ; ++j )
615 1 : (*this)( i, j ) = 0;
616 2 : return *this;
617 : }
618 :
619 : template< size_t R, size_t C, typename T >
620 2 : void Matrix< R, C, T >::operator=( const std::vector< T >& values )
621 : {
622 2 : const size_t to = std::min( R * C, values.size( ));
623 2 : ::memcpy( array, values.data(), to * sizeof( T ));
624 2 : if( to < R * C )
625 : {
626 : #ifdef _WIN32
627 : ::memset( array + to, 0, (R * C - to) * sizeof( T ));
628 : #else
629 0 : ::bzero( array + to, (R * C - to) * sizeof( T ));
630 : #endif
631 : }
632 2 : }
633 :
634 : template< size_t R, size_t C, typename T > template< size_t P >
635 : Matrix< R, C, T >& Matrix< R, C, T >::multiply( const Matrix< R, P, T >& left,
636 : const Matrix< P, C, T >& right )
637 : {
638 : // Create copy for multiplication with self
639 : if( &left == this )
640 : return multiply( Matrix< R, P, T >( left ), right );
641 : if( &right == this )
642 : return multiply( left, Matrix< R, P, T >( right ));
643 :
644 : for( size_t rowIndex = 0; rowIndex< R; ++rowIndex )
645 : {
646 : for( size_t colIndex = 0; colIndex < C; ++colIndex )
647 : {
648 : T& component = (*this)( rowIndex, colIndex );
649 : component = static_cast< T >( 0.0 );
650 : for( size_t p = 0; p < P; p++)
651 : component += left( rowIndex, p ) * right( p, colIndex );
652 : }
653 : }
654 : return *this;
655 : }
656 :
657 : template< size_t R, size_t C, typename T > template< size_t P >
658 : Matrix< R, P, T > Matrix< R, C, T >::operator*( const Matrix< C, P, T >& other )
659 : const
660 : {
661 : Matrix< R, P, T > result;
662 : return result.multiply( *this, other );
663 : }
664 :
665 : #ifdef VMMLIB_USE_CXX03
666 : template< size_t R, size_t C, typename T > template< size_t O, size_t P >
667 : typename enable_if< R == C && O == P && R == O >::type*
668 : #else
669 : template< size_t R, size_t C, typename T > template< size_t O, size_t P, typename >
670 : Matrix< R, C, T >&
671 : #endif
672 : Matrix< R, C, T >::operator*=( const Matrix< O, P, T >& right )
673 : {
674 : Matrix< R, C, T > copy( *this );
675 : multiply( copy, right );
676 : #ifdef VMMLIB_USE_CXX03
677 : return 0;
678 : #else
679 : return *this;
680 : #endif
681 : }
682 :
683 : template< size_t R, size_t C, typename T >
684 3 : vector< R, T > Matrix< R, C, T >::operator*( const vector< C, T >& vec ) const
685 : {
686 3 : vector< R, T > result;
687 :
688 : // this < R, 1 > = < R, P > * < P, 1 >
689 13 : for( size_t i = 0; i< R; ++i )
690 : {
691 10 : T tmp( 0 );
692 44 : for( size_t j = 0; j < C; ++j )
693 34 : tmp += (*this)( i, j ) * vec( j );
694 10 : result( i ) = tmp;
695 : }
696 3 : return result;
697 : }
698 :
699 : template< size_t R, size_t C, typename T > inline
700 0 : Matrix< R, C, T > Matrix< R, C, T >::operator-() const
701 : {
702 0 : Matrix< R, C, T > result;
703 0 : for( size_t i = 0; i< R * C; ++i )
704 0 : result.array[ i ] = -array[ i ];
705 0 : return result;
706 : }
707 :
708 : template< size_t R, size_t C, typename T >
709 0 : vector< R, T > Matrix< R, C, T >::getColumn( const size_t index ) const
710 : {
711 0 : if ( index >= C )
712 0 : throw std::runtime_error( "getColumn() - index out of bounds." );
713 0 : vector< R, T > column;
714 0 : ::memcpy( &column.array[0], &array[ R * index ], R * sizeof( T ));
715 0 : return column;
716 : }
717 :
718 : template< size_t R, size_t C, typename T >
719 0 : void Matrix< R, C, T >::setColumn( size_t index, const vector< R, T >& column )
720 : {
721 0 : if ( index >= C )
722 0 : throw std::runtime_error( "setColumn() - index out of bounds." );
723 0 : memcpy( array + R * index, column.array, R * sizeof( T ) );
724 0 : }
725 :
726 : template< size_t R, size_t C, typename T >
727 4 : vector< C, T > Matrix< R, C, T >::getRow( size_t index ) const
728 : {
729 4 : if ( index >= R )
730 0 : throw std::runtime_error( "getRow() - index out of bounds." );
731 :
732 4 : vector< C, T > row;
733 20 : for( size_t colIndex = 0; colIndex < C; ++colIndex )
734 16 : row( colIndex ) = (*this)( index, colIndex );
735 4 : return row;
736 : }
737 :
738 : template< size_t R, size_t C, typename T >
739 0 : void Matrix< R, C, T >::setRow( size_t rowIndex, const vector< C, T >& row )
740 : {
741 0 : if ( rowIndex >= R )
742 0 : throw std::runtime_error( "setRow() - index out of bounds." );
743 :
744 0 : for( size_t colIndex = 0; colIndex < C; ++colIndex )
745 0 : (*this)( rowIndex, colIndex ) = row( colIndex );
746 0 : }
747 :
748 : template< size_t R, size_t C, typename T > inline
749 0 : Matrix< R, C, T > Matrix< R, C, T >::operator+( const Matrix< R, C, T >& other )
750 : const
751 : {
752 0 : Matrix< R, C, T > result( *this );
753 0 : result += other;
754 0 : return result;
755 : }
756 :
757 : template< size_t R, size_t C, typename T >
758 0 : void Matrix< R, C, T >::operator+=( const Matrix< R, C, T >& other )
759 : {
760 0 : for( size_t i = 0; i < R * C; ++i )
761 0 : array[i] += other.array[i];
762 0 : }
763 :
764 : template< size_t R, size_t C, typename T > inline
765 0 : Matrix< R, C, T > Matrix< R, C, T >::operator-( const Matrix< R, C, T >& other )
766 : const
767 : {
768 0 : Matrix< R, C, T > result( *this );
769 0 : result -= other;
770 0 : return result;
771 : }
772 :
773 : template< size_t R, size_t C, typename T >
774 0 : void Matrix< R, C, T >::operator-=( const Matrix< R, C, T >& other )
775 : {
776 0 : for( size_t i = 0; i < R * C; ++i )
777 0 : array[i] -= other.array[i];
778 0 : }
779 :
780 : template< size_t R, size_t C, typename T > template< size_t O, size_t P >
781 : Matrix< O, P, T > Matrix< R, C, T >::getSubMatrix( size_t rowOffset,
782 : size_t colOffset,
783 : typename enable_if< O <= R && P <= C >::type* ) const
784 : {
785 : Matrix< O, P, T > result;
786 : if ( O + rowOffset > R || P + colOffset > C )
787 : throw std::runtime_error( "index out of bounds." );
788 :
789 : for( size_t row = 0; row < O; ++row )
790 : for( size_t col = 0; col < P; ++col )
791 : result( row, col ) = (*this)( rowOffset + row, colOffset + col );
792 :
793 : return result;
794 : }
795 :
796 : template< size_t R, size_t C, typename T > template< size_t O, size_t P >
797 : typename enable_if< O <= R && P <= C >::type*
798 : Matrix< R, C, T >::setSubMatrix( const Matrix< O, P, T >& sub_matrix,
799 : size_t rowOffset, size_t colOffset )
800 : {
801 : for( size_t row = 0; row < O; ++row )
802 : for( size_t col = 0; col < P; ++col )
803 : (*this)( rowOffset + row, colOffset + col ) = sub_matrix( row, col);
804 : return 0; // for sfinae
805 : }
806 :
807 : template< size_t R, size_t C, typename T >
808 5 : Matrix< R, C, T > Matrix< R, C, T >::inverse() const
809 : {
810 5 : return computeInverse( *this );
811 : }
812 :
813 : template< size_t R, size_t C, typename T > template< size_t O, size_t P >
814 : typename enable_if< O == P && R == C && O == R && R >= 2 >::type*
815 0 : Matrix< R, C, T >::getAdjugate( Matrix< O, P, T >& adjugate ) const
816 : {
817 0 : getCofactors( adjugate );
818 0 : adjugate = transpose( adjugate );
819 0 : return 0;
820 : }
821 :
822 : template< size_t R, size_t C, typename T > template< size_t O, size_t P >
823 : typename enable_if< O == P && R == C && O == R && R >= 2 >::type*
824 0 : Matrix< R, C, T >::getCofactors( Matrix< O, P, T >& cofactors ) const
825 : {
826 0 : Matrix< R-1, C-1, T > minor_;
827 :
828 0 : const size_t _negate = 1u;
829 0 : for( size_t rowIndex = 0; rowIndex< R; ++rowIndex )
830 0 : for( size_t colIndex = 0; colIndex < C; ++colIndex )
831 0 : if ( ( rowIndex + colIndex ) & _negate )
832 0 : cofactors( rowIndex, colIndex ) = -getMinor( minor_, rowIndex, colIndex );
833 : else
834 0 : cofactors( rowIndex, colIndex ) = getMinor( minor_, rowIndex, colIndex );
835 :
836 0 : return 0;
837 : }
838 :
839 : template< size_t R, size_t C, typename T > template< size_t O, size_t P >
840 0 : T Matrix< R, C, T >::getMinor( Matrix< O, P, T >& minor_, size_t row_to_cut,
841 : size_t col_to_cut,
842 : typename enable_if< O == R-1 && P == C-1 && R == C && 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 > template< typename TT >
867 : Matrix< R, C, T >& Matrix< R, C, T >::rotate_x( const TT angle_,
868 : typename enable_if< R == C && R == 4, TT >::type* )
869 : {
870 : const T angle = static_cast< T >( angle_ );
871 : const T sine = std::sin( angle );
872 : const T cosine = std::cos( angle );
873 :
874 : T tmp;
875 :
876 : tmp = array[ 4 ] * cosine + array[ 8 ] * sine;
877 : array[ 8 ] = - array[ 4 ] * sine + array[ 8 ] * cosine;
878 : array[ 4 ] = tmp;
879 :
880 : tmp = array[ 5 ] * cosine + array[ 9 ] * sine;
881 : array[ 9 ] = - array[ 5 ] * sine + array[ 9 ] * cosine;
882 : array[ 5 ] = tmp;
883 :
884 : tmp = array[ 6 ] * cosine + array[ 10 ] * sine;
885 : array[ 10 ] = - array[ 6 ] * sine + array[ 10 ] * cosine;
886 : array[ 6 ] = tmp;
887 :
888 : tmp = array[ 7 ] * cosine + array[ 11 ] * sine;
889 : array[ 11 ] = - array[ 7 ] * sine + array[ 11 ] * cosine;
890 : array[ 7 ] = tmp;
891 :
892 : return *this;
893 : }
894 :
895 : template< size_t R, size_t C, typename T > template< typename TT >
896 : Matrix< R, C, T >& Matrix< R, C, T >::rotate_y( const TT angle_,
897 : typename enable_if< R == C && R == 4, TT >::type* )
898 : {
899 : const T angle = static_cast< T >( angle_ );
900 : const T sine = std::sin( angle );
901 : const T cosine = std::cos( angle );
902 :
903 : T tmp;
904 :
905 : tmp = array[ 0 ] * cosine - array[ 8 ] * sine;
906 : array[ 8 ] = array[ 0 ] * sine + array[ 8 ] * cosine;
907 : array[ 0 ] = tmp;
908 :
909 : tmp = array[ 1 ] * cosine - array[ 9 ] * sine;
910 : array[ 9 ] = array[ 1 ] * sine + array[ 9 ] * cosine;
911 : array[ 1 ] = tmp;
912 :
913 : tmp = array[ 2 ] * cosine - array[ 10 ] * sine;
914 : array[ 10 ] = array[ 2 ] * sine + array[ 10 ] * cosine;
915 : array[ 2 ] = tmp;
916 :
917 : tmp = array[ 3 ] * cosine - array[ 11 ] * sine;
918 : array[ 11 ] = array[ 3 ] * sine + array[ 11 ] * cosine;
919 : array[ 3 ] = tmp;
920 :
921 : return *this;
922 : }
923 :
924 : template< size_t R, size_t C, typename T > template< typename TT >
925 : Matrix< R, C, T >& Matrix< R, C, T >::rotate_z( const TT angle_,
926 : typename enable_if< R == C && R == 4, TT >::type* )
927 : {
928 : const T angle = static_cast< T >( angle_ );
929 : const T sine = std::sin( angle );
930 : const T cosine = std::cos( angle );
931 :
932 : T tmp;
933 :
934 : tmp = array[ 0 ] * cosine + array[ 4 ] * sine;
935 : array[ 4 ] = - array[ 0 ] * sine + array[ 4 ] * cosine;
936 : array[ 0 ] = tmp;
937 :
938 : tmp = array[ 1 ] * cosine + array[ 5 ] * sine;
939 : array[ 5 ] = - array[ 1 ] * sine + array[ 5 ] * cosine;
940 : array[ 1 ] = tmp;
941 :
942 : tmp = array[ 2 ] * cosine + array[ 6 ] * sine;
943 : array[ 6 ] = - array[ 2 ] * sine + array[ 6 ] * cosine;
944 : array[ 2 ] = tmp;
945 :
946 : tmp = array[ 3 ] * cosine + array[ 7 ] * sine;
947 : array[ 7 ] = - array[ 3 ] * sine + array[ 7 ] * cosine;
948 : array[ 3 ] = tmp;
949 :
950 : return *this;
951 : }
952 :
953 : template< size_t R, size_t C, typename T > template< typename TT >
954 : Matrix< R, C, T >& Matrix< R, C, T >::pre_rotate_x( const TT angle_,
955 : typename enable_if< R == C && R == 4, TT >::type* )
956 : {
957 : const T angle = static_cast< T >( angle_ );
958 : const T sine = std::sin( angle );
959 : const T cosine = std::cos( angle );
960 :
961 : T tmp;
962 :
963 : tmp = array[ 1 ];
964 : array[ 1 ] = array[ 1 ] * cosine + array[ 2 ] * sine;
965 : array[ 2 ] = tmp * -sine + array[ 2 ] * cosine;
966 :
967 : tmp = array[ 5 ];
968 : array[ 5 ] = array[ 5 ] * cosine + array[ 6 ] * sine;
969 : array[ 6 ] = tmp * -sine + array[ 6 ] * cosine;
970 :
971 : tmp = array[ 9 ];
972 : array[ 9 ] = array[ 9 ] * cosine + array[ 10 ] * sine;
973 : array[ 10 ] = tmp * -sine + array[ 10 ] * cosine;
974 :
975 : tmp = array[ 13 ];
976 : array[ 13 ] = array[ 13 ] * cosine + array[ 14 ] * sine;
977 : array[ 14 ] = tmp * -sine + array[ 14 ] * cosine;
978 :
979 : return *this;
980 : }
981 :
982 : template< size_t R, size_t C, typename T > template< typename TT >
983 : Matrix< R, C, T >& Matrix< R, C, T >::pre_rotate_y( const TT angle_,
984 : typename enable_if< R == C && R == 4, TT >::type* )
985 : {
986 : const T angle = static_cast< T >( angle_ );
987 : const T sine = std::sin( angle );
988 : const T cosine = std::cos( angle );
989 :
990 : T tmp;
991 :
992 : tmp = array[ 0 ];
993 : array[ 0 ] = array[ 0 ] * cosine - array[ 2 ] * sine;
994 : array[ 2 ] = tmp * sine + array[ 2 ] * cosine;
995 :
996 : tmp = array[ 4 ];
997 : array[ 4 ] = array[ 4 ] * cosine - array[ 6 ] * sine;
998 : array[ 6 ] = tmp * sine + array[ 6 ] * cosine;
999 :
1000 : tmp = array[ 8 ];
1001 : array[ 8 ] = array[ 8 ] * cosine - array[ 10 ] * sine;
1002 : array[ 10 ] = tmp * sine + array[ 10 ] * cosine;
1003 :
1004 : tmp = array[ 12 ];
1005 : array[ 12 ] = array[ 12 ] * cosine - array[ 14 ] * sine;
1006 : array[ 14 ] = tmp * sine + array[ 14 ] * cosine;
1007 :
1008 : return *this;
1009 : }
1010 :
1011 : template< size_t R, size_t C, typename T > template< typename TT >
1012 : Matrix< R, C, T >& Matrix< R, C, T >::pre_rotate_z( const TT angle_,
1013 : typename enable_if< R == C && R == 4, TT >::type* )
1014 : {
1015 : const T angle = static_cast< T >( angle_ );
1016 : const T sine = std::sin( angle );
1017 : const T cosine = std::cos( angle );
1018 :
1019 : T tmp;
1020 :
1021 : tmp = array[ 0 ];
1022 : array[ 0 ] = array[ 0 ] * cosine + array[ 1 ] * sine;
1023 : array[ 1 ] = tmp * -sine + array[ 1 ] * cosine;
1024 :
1025 : tmp = array[ 4 ];
1026 : array[ 4 ] = array[ 4 ] * cosine + array[ 5 ] * sine;
1027 : array[ 5 ] = tmp * -sine + array[ 5 ] * cosine;
1028 :
1029 : tmp = array[ 8 ];
1030 : array[ 8 ] = array[ 8 ] * cosine + array[ 9 ] * sine;
1031 : array[ 9 ] = tmp * -sine + array[ 9 ] * cosine;
1032 :
1033 : tmp = array[ 12 ];
1034 : array[ 12 ] = array[ 12 ] * cosine + array[ 13 ] * sine;
1035 : array[ 13 ] = tmp * -sine + array[ 13 ] * cosine;
1036 :
1037 : return *this;
1038 : }
1039 :
1040 : template< size_t R, size_t C, typename T > template< typename TT > inline
1041 : Matrix< R, C, T >& Matrix< R, C, T >::scale( const vector< 3, TT >& scale_,
1042 : typename enable_if< R == C && R == 4, TT >::type* )
1043 : {
1044 : array[0] *= scale_[ 0 ];
1045 : array[1] *= scale_[ 0 ];
1046 : array[2] *= scale_[ 0 ];
1047 : array[3] *= scale_[ 0 ];
1048 : array[4] *= scale_[ 1 ];
1049 : array[5] *= scale_[ 1 ];
1050 : array[6] *= scale_[ 1 ];
1051 : array[7] *= scale_[ 1 ];
1052 : array[8] *= scale_[ 2 ];
1053 : array[9] *= scale_[ 2 ];
1054 : array[10] *= scale_[ 2 ];
1055 : array[11] *= scale_[ 2 ];
1056 : return *this;
1057 : }
1058 :
1059 :
1060 : template< size_t R, size_t C, typename T > template< typename TT > inline
1061 : Matrix< R, C, T >& Matrix< R, C, T >::scaleTranslation(
1062 : const vector< 3, TT >& scale_,
1063 : typename enable_if< R == C && R == 4, TT >::type* )
1064 : {
1065 : array[12] *= static_cast< T >( scale_[0] );
1066 : array[13] *= static_cast< T >( scale_[1] );
1067 : array[14] *= static_cast< T >( scale_[2] );
1068 : return *this;
1069 : }
1070 :
1071 :
1072 : template< size_t R, size_t C, typename T > inline Matrix< R, C, T >&
1073 1 : Matrix< R, C, T >::setTranslation( const vector< C-1, T >& trans )
1074 : {
1075 4 : for( size_t i = 0; i < C-1; ++i )
1076 3 : array[ i + R * (C - 1) ] = trans[ i ];
1077 1 : return *this;
1078 : }
1079 :
1080 : template< size_t R, size_t C, typename T > inline
1081 0 : vector< C-1, T > Matrix< R, C, T >::getTranslation() const
1082 : {
1083 0 : vector< C-1, T > result;
1084 0 : for( size_t i = 0; i < C-1; ++i )
1085 0 : result[ i ] = array[ i + R * (C - 1) ];
1086 0 : return result;
1087 : }
1088 :
1089 : template< size_t R, size_t C, typename T > template< size_t S >
1090 1 : void Matrix< R, C, T >::getLookAt( vector< S, T >& eye, vector< S, T >& lookAt,
1091 : vector< S, T >& up,
1092 : typename enable_if< R == S+1 && C == S+1 && S == 3 >::type* ) const
1093 : {
1094 1 : const Matrix< 4, 4, T >& inv = inverse();
1095 1 : const Matrix< 3, 3, T > rotation( transpose( Matrix< 3, 3, T >( *this )));
1096 :
1097 1 : eye = vector< S, T >( inv * vector< 4, T >::zero( ));
1098 1 : up = rotation * vector< 3, T >::up();
1099 1 : lookAt = rotation * vector< 3, T >::forward();
1100 1 : lookAt.normalize();
1101 1 : lookAt = eye + lookAt;
1102 1 : }
1103 :
1104 : } // namespace vmml
1105 :
1106 : #endif
|
