Cinder  0.8.6
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Pages
Matrix44.h
Go to the documentation of this file.
1 /*
2  Copyright (c) 2011, The Cinder Project: http://libcinder.org All rights reserved.
3  This code is intended for use with the Cinder C++ library: http://libcinder.org
4 
5  Redistribution and use in source and binary forms, with or without modification, are permitted provided that
6  the following conditions are met:
7 
8  * Redistributions of source code must retain the above copyright notice, this list of conditions and
9  the following disclaimer.
10  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
11  the following disclaimer in the documentation and/or other materials provided with the distribution.
12 
13  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
14  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
15  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
16  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
17  TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
18  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
19  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
20  POSSIBILITY OF SUCH DAMAGE.
21 */
22 
23 
24 #pragma once
25 
26 #include "cinder/Cinder.h"
27 #include "cinder/CinderMath.h"
28 #include "cinder/Vector.h"
29 
30 #include "cinder/Matrix22.h"
31 #include "cinder/Matrix33.h"
32 #include "cinder/MatrixAffine2.h"
33 
34 #include <iomanip>
35 
36 namespace cinder {
37 
39 // Matrix44
40 template< typename T >
41 class Matrix44
42 {
43 public:
44  typedef T TYPE;
45  typedef T value_type;
46  //
47  static const size_t DIM = 4;
48  static const size_t DIM_SQ = DIM*DIM;
49  static const size_t MEM_LEN = sizeof(T)*DIM_SQ;
50 
51  //
52  // This class is OpenGL friendly and stores the m as how OpenGL would expect it.
53  // m[i,j]:
54  // | m[0,0] m[0,1] m[0,2] m[0,2] |
55  // | m[1,0] m[1,1] m[1,2] m[1,2] |
56  // | m[2,0] m[2,1] m[2,2] m[2,2] |
57  // | m[3,0] m[3,1] m[3,2] m[3,2] |
58  //
59  // m[idx]
60  // | m[ 0] m[ 4] m[ 8] m[12] |
61  // | m[ 1] m[ 5] m[ 9] m[13] |
62  // | m[ 2] m[ 6] m[10] m[14] |
63  // | m[ 3] m[ 7] m[11] m[15] |
64  //
65  union {
66  T m[16];
67  struct {
68  // This looks like it's transposed from the above, but it's really not.
69  // It just has to be written this way so it follows the right ordering
70  // in the memory layout as well as being mathematically correct.
71  T m00, m10, m20, m30;
72  T m01, m11, m21, m31;
73  T m02, m12, m22, m32;
74  T m03, m13, m23, m33;
75  };
76  // [Cols][Rows]
77  T mcols[4][4];
78  };
79 
80  Matrix44();
81 
82  Matrix44( T s );
83 
84  // OpenGL layout - unless srcIsRowMajor is true
85  Matrix44( const T *dt, bool srcIsRowMajor = false );
86 
87  // OpenGL layout: m[0]=d0, m[1]=d1, m[2]=d2 ... m[13]=d13, m[14]=d14, m[15]=d15 - unless srcIsRowMajor is true
88  Matrix44( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, T d9, T d10, T d11, T d12, T d13, T d14, T d15, bool srcIsRowMajor = false );
89 
90  // Creates matrix with column vectors vx, vy, vz and vw
91  Matrix44( const Vec3<T> &vx, const Vec3<T> &vy, const Vec3<T> &vz );
92  Matrix44( const Vec4<T> &vx, const Vec4<T> &vy, const Vec4<T> &vz, const Vec4<T> &vw = Vec4<T>( 0, 0, 0, 1 ) );
93 
94  template< typename FromT >
95  Matrix44( const Matrix44<FromT>& src );
96 
97  Matrix44( const Matrix22<T>& src );
98  explicit Matrix44( const MatrixAffine2<T> &src );
99  Matrix44( const Matrix33<T>& src );
100 
101  Matrix44( const Matrix44<T>& src );
102 
103  operator T*() { return (T*)m; }
104  operator const T*() const { return (const T*)m; }
105 
106  Matrix44<T>& operator=( const Matrix44<T>& rhs );
107  Matrix44<T>& operator=( T rhs );
108 
109  template< typename FromT >
110  Matrix44<T>& operator=( const Matrix44<FromT>& rhs );
111 
112  // remaining columns and rows will be filled with identity values
113  Matrix44<T>& operator=( const Matrix22<T>& rhs );
114  Matrix44<T>& operator=( const MatrixAffine2<T>& rhs );
115  Matrix44<T>& operator=( const Matrix33<T>& rhs );
116 
117  bool equalCompare( const Matrix44<T>& rhs, T epsilon ) const;
118  bool operator==( const Matrix44<T> &rhs ) const { return equalCompare( rhs, (T)EPSILON ); }
119  bool operator!=( const Matrix44<T> &rhs ) const { return ! ( *this == rhs ); }
120 
121  Matrix44<T>& operator*=( const Matrix44<T> &rhs );
122  Matrix44<T>& operator+=( const Matrix44<T> &rhs );
123  Matrix44<T>& operator-=( const Matrix44<T> &rhs );
124 
125  Matrix44<T>& operator*=( T rhs );
126  Matrix44<T>& operator/=( T rhs );
127  Matrix44<T>& operator+=( T rhs );
128  Matrix44<T>& operator-=( T rhs );
129 
130  const Matrix44<T> operator*( const Matrix44<T> &rhs ) const;
131  const Matrix44<T> operator+( const Matrix44<T> &rhs ) const;
132  const Matrix44<T> operator-( const Matrix44<T> &rhs ) const;
133 
134  // post-multiplies column vector [rhs.x rhs.y rhs.z 1] and divides by w
135  const Vec3<T> operator*( const Vec3<T> &rhs ) const;
136 
137  // post-multiplies column vector [rhs.x rhs.y rhs.z rhs.w]
138  const Vec4<T> operator*( const Vec4<T> &rhs ) const;
139 
140  const Matrix44<T> operator*( T rhs ) const;
141  const Matrix44<T> operator/( T rhs ) const;
142  const Matrix44<T> operator+( T rhs ) const;
143  const Matrix44<T> operator-( T rhs ) const;
144 
145  // Accessors
146  T& at( int row, int col );
147  const T& at( int row, int col ) const;
148 
149  // OpenGL layout - unless srcIsRowMajor is true
150  void set( const T *dt, bool srcIsRowMajor = false );
151  // OpenGL layout: m[0]=d0, m[1]=d1, m[2]=d2 ... m[13]=d13, m[14]=d14, m[15]=d15 - unless srcIsRowMajor is true
152  void set( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, T d9, T d10, T d11, T d12, T d13, T d14, T d15, bool srcIsRowMajor = false );
153 
154  Vec4<T> getColumn( int col ) const;
155  void setColumn( int col, const Vec4<T> &v );
156 
157  Vec4<T> getRow( int row ) const;
158  void setRow( int row, const Vec4<T> &v );
159 
160  void getColumns( Vec4<T> *c0, Vec4<T> *c1, Vec4<T> *c2, Vec4<T> *c3 ) const;
161  void setColumns( const Vec4<T> &c0, const Vec4<T> &c1, const Vec4<T> &c2, const Vec4<T> &c3 );
162 
163  void getRows( Vec4<T> *r0, Vec4<T> *r1, Vec4<T> *r2, Vec4<T> *r3 ) const;
164  void setRows( const Vec4<T> &r0, const Vec4<T> &r1, const Vec4<T> &r2, const Vec4<T> &r3 );
165 
166  // returns a sub-matrix starting at row, col
167  Matrix22<T> subMatrix22( int row, int col ) const;
168  Matrix33<T> subMatrix33( int row, int col ) const;
169 
170  void setToNull();
171  void setToIdentity();
172 
173  T determinant() const;
174  // returns trace of 3x3 submatrix if fullTrace == false, otherwise returns trace of full 4x4 matrix
175  T trace( bool fullTrace = false ) const;
176 
177  Matrix44<T> diagonal() const;
178 
181 
182  void transpose();
183  Matrix44<T> transposed() const;
184 
185  void invert (T epsilon = FLT_MIN ) { *this = inverted( epsilon ); }
186  Matrix44<T> inverted( T epsilon = FLT_MIN ) const;
187 
188  // pre-multiplies row vector v - no divide by w
189  Vec3<T> preMultiply( const Vec3<T> &v ) const;
190  Vec4<T> preMultiply( const Vec4<T> &v ) const;
191 
192  // post-multiplies column vector v - no divide by w
193  Vec3<T> postMultiply( const Vec3<T> &v ) const;
194  Vec4<T> postMultiply( const Vec4<T> &v ) const;
195 
197  void affineInvert(){ *this = affineInverted(); }
198  Matrix44<T> affineInverted() const;
199 
201  void orthonormalInvert();
202  Matrix44<T> orthonormalInverted() const { Matrix44<T> result( *this ); result.orthonormalInvert(); return result; }
203 
204  // post-multiplies column vector [rhs.x rhs.y rhs.z 1] and divides by w - same as operator*( const Vec3<T>& )
205  Vec3<T> transformPoint( const Vec3<T> &rhs ) const;
206 
207  // post-multiplies column vector [rhs.x rhs.y rhs.z 1] but omits divide by w
208  Vec3<T> transformPointAffine( const Vec3<T> &rhs ) const;
209 
210  // post-multiplies column vector [rhs.x rhs.y rhs.z 0]
211  Vec3<T> transformVec( const Vec3<T> &rhs ) const;
212  Vec4<T> transformVec( const Vec4<T> &rhs ) const { return transformVec( rhs.xyz() ); }
213 
214  // returns the translation values from the last column
215  Vec4<T> getTranslate() const { return Vec4<T>( m03, m13, m23, m33 ); }
216  // sets the translation values in the last column
217  void setTranslate( const Vec3<T>& v ) { m03 = v.x; m13 = v.y; m23 = v.z; }
218  void setTranslate( const Vec4<T>& v ) { setTranslate( v.xyz() ); }
219 
220  // multiplies the current matrix by a translation matrix derived from tr
221  void translate( const Vec3<T> &tr ) { *this *= createTranslation( tr ); }
222  void translate( const Vec4<T> &tr ) { *this *= createTranslation( tr ); }
223 
224  // multiplies the current matrix by the rotation matrix derived using axis and radians
225  void rotate( const Vec3<T> &axis, T radians ) { *this *= createRotation( axis, radians ); }
226  void rotate( const Vec4<T> &axis, T radians ) { *this *= createRotation( axis, radians ); }
227  // multiplies the current matrix by the rotation matrix derived using eulerRadians
228  void rotate( const Vec3<T> &eulerRadians ) { *this *= createRotation( eulerRadians ); }
229  void rotate( const Vec4<T> &eulerRadians ) { *this *= createRotation( eulerRadians ); }
230  // multiplies the current matrix by the rotation matrix derived using from, to, worldUp
231  void rotate( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp ) { *this *= createRotation( from, to, worldUp ); }
232  void rotate( const Vec4<T> &from, const Vec4<T> &to, const Vec4<T> &worldUp ) { *this *= createRotation( from, to, worldUp ); }
233 
234  // multiplies the current matrix by the scale matrix derived from supplies parameters
235  void scale( T s ) { Matrix44 op = createScale( s ); Matrix44 mat = *this; *this = op*mat; }
236  void scale( const Vec2<T> &v ) { *this *= createScale( v ); }
237  void scale( const Vec3<T> &v ) { *this *= createScale( v ); }
238  void scale( const Vec4<T> &v ) { *this *= createScale( v ); }
239 
240  // transposes rotation sub-matrix and inverts translation
242 
243  // returns an identity matrix
244  static Matrix44<T> identity() { return Matrix44( 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ); }
245  // returns 1 filled matrix
246  static Matrix44<T> one() { return Matrix44( (T)1 ); }
247  // returns 0 filled matrix
248  static Matrix44<T> zero() { return Matrix44( (T)0 ); }
249 
250  // creates translation matrix
251  static Matrix44<T> createTranslation( const Vec3<T> &v, T w = 1 );
252  static Matrix44<T> createTranslation( const Vec4<T> &v ) { return createTranslation( v.xyz(), v.w );}
253 
254  // creates rotation matrix
255  static Matrix44<T> createRotation( const Vec3<T> &axis, T radians );
256  static Matrix44<T> createRotation( const Vec4<T> &axis, T radians ) { return createRotation( axis.xyz(), radians ); }
257  static Matrix44<T> createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp );
258  static Matrix44<T> createRotation( const Vec4<T> &from, const Vec4<T> &to, const Vec4<T> &worldUp ) { return createRotation( from.xyz(), to.xyz(), worldUp.xyz() ); }
259  // equivalent to rotate( zAxis, z ), then rotate( yAxis, y ) then rotate( xAxis, x )
260  static Matrix44<T> createRotation( const Vec3<T> &eulerRadians );
261  static Matrix44<T> createRotation( const Vec4<T> &eulerRadians ) { return createRotation( eulerRadians.xyz() ); }
262  // creates rotation matrix from ortho normal basis (u, v, n)
263  static Matrix44<T> createRotationOnb( const Vec3<T>& u, const Vec3<T>& v, const Vec3<T>& w );
264  static Matrix44<T> createRotationOnb( const Vec4<T>& u, const Vec4<T>& v, const Vec4<T>& w ) { return createRotationOnb( u.xyz(), v.xyz(), w.xyz() ); }
265 
266  // creates scale matrix
267  static Matrix44<T> createScale( T s );
268  static Matrix44<T> createScale( const Vec2<T> &v );
269  static Matrix44<T> createScale( const Vec3<T> &v );
270  static Matrix44<T> createScale( const Vec4<T> &v );
271 
272  // creates a rotation matrix with z-axis aligned to targetDir
273  static Matrix44<T> alignZAxisWithTarget( Vec3<T> targetDir, Vec3<T> upDir );
274  static Matrix44<T> alignZAxisWithTarget( Vec4<T> targetDir, Vec4<T> upDir ) { return alignZAxisWithTarget( targetDir.xyz(), upDir.xyz() ); }
275 
276  friend std::ostream& operator<<( std::ostream &lhs, const Matrix44<T> &rhs ) {
277  for( int i = 0; i < 4; i++) {
278  lhs << " |";
279  for( int j = 0; j < 4; j++) {
280  lhs << std::setw( 12 ) << std::setprecision( 6 ) << rhs.m[j*4+i];
281  }
282  lhs << "|" << std::endl;
283  }
284 
285  return lhs;
286  }
287 };
288 
289 template< typename T >
291 {
292  setToIdentity();
293 }
294 
295 template< typename T >
297 {
298  for( int i = 0; i < DIM_SQ; ++i ) {
299  m[i] = s;
300  }
301 }
302 
303 template< typename T >
304 Matrix44<T>::Matrix44( const T *dt, bool srcIsRowMajor )
305 {
306  set( dt, srcIsRowMajor );
307 }
308 
309 template< typename T >
310 Matrix44<T>::Matrix44( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, T d9, T d10, T d11, T d12, T d13, T d14, T d15, bool srcIsRowMajor )
311 {
312  set( d0, d1, d2, d3,
313  d4, d5, d6, d7,
314  d8, d9, d10, d11,
315  d12, d13, d14, d15, srcIsRowMajor );
316 }
317 
318 template< typename T >
319 Matrix44<T>::Matrix44( const Vec3<T> &vx, const Vec3<T> &vy, const Vec3<T> &vz )
320 {
321  m[0] = vx.x; m[4] = vy.x; m[ 8] = vz.x; m[12] = 0;
322  m[1] = vx.y; m[5] = vy.y; m[ 9] = vz.y; m[13] = 0;
323  m[2] = vx.z; m[6] = vy.z; m[10] = vz.z; m[14] = 0;
324  m[3] = 0; m[7] = 0; m[11] = 0; m[15] = 1;
325 }
326 
327 template< typename T >
328 Matrix44<T>::Matrix44( const Vec4<T> &vx, const Vec4<T> &vy, const Vec4<T> &vz, const Vec4<T> &vw )
329 {
330  m[0] = vx.x; m[4] = vy.x; m[ 8] = vz.x; m[12] = vw.x;
331  m[1] = vx.y; m[5] = vy.y; m[ 9] = vz.y; m[13] = vw.y;
332  m[2] = vx.z; m[6] = vy.z; m[10] = vz.z; m[14] = vw.z;
333  m[3] = vx.w; m[7] = vy.w; m[11] = vz.w; m[15] = vw.w;
334 }
335 
336 template< typename T >
337 template< typename FromT >
339 {
340  for( int i = 0; i < DIM_SQ; ++i ) {
341  m[i] = static_cast<T>( src.m[i] );
342  }
343 }
344 
345 template< typename T >
347 {
348  setToIdentity();
349  m00 = src.m00; m01 = src.m01;
350  m10 = src.m10; m11 = src.m11;
351 }
352 
353 template< typename T >
355 {
356  m[ 0] = src.m[0]; m[ 4] = src.m[2]; m[ 8] = 0; m[12] = src.m[4];
357  m[ 1] = src.m[1]; m[ 5] = src.m[3]; m[ 9] = 0; m[13] = src.m[5];
358  m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0;
359  m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
360 }
361 
362 template< typename T >
364 {
365  setToIdentity();
366  m00 = src.m00; m01 = src.m01; m02 = src.m02;
367  m10 = src.m10; m11 = src.m11; m12 = src.m12;
368  m20 = src.m20; m21 = src.m21; m22 = src.m22;
369 }
370 
371 template< typename T >
373 {
374  std::memcpy( m, src.m, MEM_LEN );
375 }
376 
377 template< typename T >
379 {
380  std::memcpy( m, rhs.m, MEM_LEN );
381  return *this;
382 }
383 
384 template< typename T >
386 {
387  for( int i = 0; i < DIM_SQ; i++ ) {
388  m[i] = rhs;
389  }
390  return *this;
391 }
392 
393 template< typename T >
394 template< typename FromT >
396 {
397  for( int i = 0; i < DIM_SQ; i++ ) {
398  m[i] = static_cast<T>(rhs.m[i]);
399  }
400  return *this;
401 }
402 
403 template< typename T >
405 {
406  setToIdentity();
407  m00 = rhs.m00; m01 = rhs.m01;
408  m10 = rhs.m10; m11 = rhs.m11;
409  return *this;
410 }
411 
412 template< typename T >
414 {
415  m[ 0] = rhs.m[0]; m[ 4] = rhs.m[2]; m[ 8] = 0; m[12] = rhs.m[4];
416  m[ 1] = rhs.m[1]; m[ 5] = rhs.m[3]; m[ 9] = 0; m[13] = rhs.m[5];
417  m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0;
418  m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
419  return *this;
420 }
421 
422 
423 
424 template< typename T >
426 {
427  setToIdentity();
428  m00 = rhs.m00; m01 = rhs.m01; m02 = rhs.m02;
429  m10 = rhs.m10; m11 = rhs.m11; m12 = rhs.m12;
430  m20 = rhs.m20; m21 = rhs.m21; m22 = rhs.m22;
431  return *this;
432 }
433 
434 template< typename T >
435 bool Matrix44<T>::equalCompare( const Matrix44<T>& rhs, T epsilon ) const
436 {
437  for( int i = 0; i < DIM_SQ; ++i ) {
438  if( math<T>::abs(m[i] - rhs.m[i]) >= epsilon )
439  return false;
440  }
441  return true;
442 }
443 
444 template< typename T >
446 {
447  Matrix44<T> ret;
448 
449  ret.m[ 0] = m[ 0]*rhs.m[ 0] + m[ 4]*rhs.m[ 1] + m[ 8]*rhs.m[ 2] + m[12]*rhs.m[ 3];
450  ret.m[ 1] = m[ 1]*rhs.m[ 0] + m[ 5]*rhs.m[ 1] + m[ 9]*rhs.m[ 2] + m[13]*rhs.m[ 3];
451  ret.m[ 2] = m[ 2]*rhs.m[ 0] + m[ 6]*rhs.m[ 1] + m[10]*rhs.m[ 2] + m[14]*rhs.m[ 3];
452  ret.m[ 3] = m[ 3]*rhs.m[ 0] + m[ 7]*rhs.m[ 1] + m[11]*rhs.m[ 2] + m[15]*rhs.m[ 3];
453 
454  ret.m[ 4] = m[ 0]*rhs.m[ 4] + m[ 4]*rhs.m[ 5] + m[ 8]*rhs.m[ 6] + m[12]*rhs.m[ 7];
455  ret.m[ 5] = m[ 1]*rhs.m[ 4] + m[ 5]*rhs.m[ 5] + m[ 9]*rhs.m[ 6] + m[13]*rhs.m[ 7];
456  ret.m[ 6] = m[ 2]*rhs.m[ 4] + m[ 6]*rhs.m[ 5] + m[10]*rhs.m[ 6] + m[14]*rhs.m[ 7];
457  ret.m[ 7] = m[ 3]*rhs.m[ 4] + m[ 7]*rhs.m[ 5] + m[11]*rhs.m[ 6] + m[15]*rhs.m[ 7];
458 
459  ret.m[ 8] = m[ 0]*rhs.m[ 8] + m[ 4]*rhs.m[ 9] + m[ 8]*rhs.m[10] + m[12]*rhs.m[11];
460  ret.m[ 9] = m[ 1]*rhs.m[ 8] + m[ 5]*rhs.m[ 9] + m[ 9]*rhs.m[10] + m[13]*rhs.m[11];
461  ret.m[10] = m[ 2]*rhs.m[ 8] + m[ 6]*rhs.m[ 9] + m[10]*rhs.m[10] + m[14]*rhs.m[11];
462  ret.m[11] = m[ 3]*rhs.m[ 8] + m[ 7]*rhs.m[ 9] + m[11]*rhs.m[10] + m[15]*rhs.m[11];
463 
464  ret.m[12] = m[ 0]*rhs.m[12] + m[ 4]*rhs.m[13] + m[ 8]*rhs.m[14] + m[12]*rhs.m[15];
465  ret.m[13] = m[ 1]*rhs.m[12] + m[ 5]*rhs.m[13] + m[ 9]*rhs.m[14] + m[13]*rhs.m[15];
466  ret.m[14] = m[ 2]*rhs.m[12] + m[ 6]*rhs.m[13] + m[10]*rhs.m[14] + m[14]*rhs.m[15];
467  ret.m[15] = m[ 3]*rhs.m[12] + m[ 7]*rhs.m[13] + m[11]*rhs.m[14] + m[15]*rhs.m[15];
468 
469  for( int i = 0; i < DIM_SQ; ++i ) {
470  m[i] = ret.m[i];
471  }
472 
473  return *this;
474 }
475 
476 template< typename T >
478 {
479  for( int i = 0; i < DIM_SQ; ++i ) {
480  m[i] += rhs.m[i];
481  }
482  return *this;
483 }
484 
485 template< typename T >
487 {
488  for( int i = 0; i < DIM_SQ; ++i ) {
489  m[i] -= rhs.m[i];
490  }
491  return *this;
492 }
493 
494 template< typename T >
496 {
497  for( int i = 0; i < DIM_SQ; ++i ) {
498  m[i] *= rhs;
499  }
500  return *this;
501 }
502 
503 template< typename T >
505 {
506  T invS = (T)1/rhs;
507  for( int i = 0; i < DIM_SQ; ++i ) {
508  m[i] *= invS;
509  }
510  return *this;
511 }
512 
513 template< typename T >
515 {
516  for( int i = 0; i < DIM_SQ; ++i ) {
517  m[i] += rhs;
518  }
519  return *this;
520 }
521 
522 template< typename T >
524 {
525  for( int i = 0; i < DIM_SQ; ++i ) {
526  m[i] -= rhs;
527  }
528  return *this;
529 }
530 
531 template< typename T >
532 inline const Matrix44<T> Matrix44<T>::operator*( const Matrix44<T> &rhs ) const
533 {
534  Matrix44<T> ret;
535 
536  ret.m[ 0] = m[ 0]*rhs.m[ 0] + m[ 4]*rhs.m[ 1] + m[ 8]*rhs.m[ 2] + m[12]*rhs.m[ 3];
537  ret.m[ 1] = m[ 1]*rhs.m[ 0] + m[ 5]*rhs.m[ 1] + m[ 9]*rhs.m[ 2] + m[13]*rhs.m[ 3];
538  ret.m[ 2] = m[ 2]*rhs.m[ 0] + m[ 6]*rhs.m[ 1] + m[10]*rhs.m[ 2] + m[14]*rhs.m[ 3];
539  ret.m[ 3] = m[ 3]*rhs.m[ 0] + m[ 7]*rhs.m[ 1] + m[11]*rhs.m[ 2] + m[15]*rhs.m[ 3];
540 
541  ret.m[ 4] = m[ 0]*rhs.m[ 4] + m[ 4]*rhs.m[ 5] + m[ 8]*rhs.m[ 6] + m[12]*rhs.m[ 7];
542  ret.m[ 5] = m[ 1]*rhs.m[ 4] + m[ 5]*rhs.m[ 5] + m[ 9]*rhs.m[ 6] + m[13]*rhs.m[ 7];
543  ret.m[ 6] = m[ 2]*rhs.m[ 4] + m[ 6]*rhs.m[ 5] + m[10]*rhs.m[ 6] + m[14]*rhs.m[ 7];
544  ret.m[ 7] = m[ 3]*rhs.m[ 4] + m[ 7]*rhs.m[ 5] + m[11]*rhs.m[ 6] + m[15]*rhs.m[ 7];
545 
546  ret.m[ 8] = m[ 0]*rhs.m[ 8] + m[ 4]*rhs.m[ 9] + m[ 8]*rhs.m[10] + m[12]*rhs.m[11];
547  ret.m[ 9] = m[ 1]*rhs.m[ 8] + m[ 5]*rhs.m[ 9] + m[ 9]*rhs.m[10] + m[13]*rhs.m[11];
548  ret.m[10] = m[ 2]*rhs.m[ 8] + m[ 6]*rhs.m[ 9] + m[10]*rhs.m[10] + m[14]*rhs.m[11];
549  ret.m[11] = m[ 3]*rhs.m[ 8] + m[ 7]*rhs.m[ 9] + m[11]*rhs.m[10] + m[15]*rhs.m[11];
550 
551  ret.m[12] = m[ 0]*rhs.m[12] + m[ 4]*rhs.m[13] + m[ 8]*rhs.m[14] + m[12]*rhs.m[15];
552  ret.m[13] = m[ 1]*rhs.m[12] + m[ 5]*rhs.m[13] + m[ 9]*rhs.m[14] + m[13]*rhs.m[15];
553  ret.m[14] = m[ 2]*rhs.m[12] + m[ 6]*rhs.m[13] + m[10]*rhs.m[14] + m[14]*rhs.m[15];
554  ret.m[15] = m[ 3]*rhs.m[12] + m[ 7]*rhs.m[13] + m[11]*rhs.m[14] + m[15]*rhs.m[15];
555 
556  return ret;
557 }
558 
559 template< typename T >
561 {
562  Matrix44<T> ret;
563  for( int i = 0; i < DIM_SQ; ++i ) {
564  ret.m[i] = m[i] + rhs.m[i];
565  }
566  return ret;
567 }
568 
569 template< typename T >
571 {
572  Matrix44<T> ret;
573  for( int i = 0; i < DIM_SQ; ++i ) {
574  ret.m[i] = m[i] - rhs.m[i];
575  }
576  return ret;
577 }
578 
579 template< typename T >
580 const Vec3<T> Matrix44<T>::operator*( const Vec3<T> &rhs ) const
581 {
582  T x = m[ 0]*rhs.x + m[ 4]*rhs.y + m[ 8]*rhs.z + m[12];
583  T y = m[ 1]*rhs.x + m[ 5]*rhs.y + m[ 9]*rhs.z + m[13];
584  T z = m[ 2]*rhs.x + m[ 6]*rhs.y + m[10]*rhs.z + m[14];
585  T w = m[ 3]*rhs.x + m[ 7]*rhs.y + m[11]*rhs.z + m[15];
586 
587  return Vec3<T>( x/w, y/w, z/w );
588 }
589 
590 template< typename T >
591 const Vec4<T> Matrix44<T>::operator*( const Vec4<T> &rhs ) const
592 {
593  return Vec4<T>(
594  m[ 0]*rhs.x + m[ 4]*rhs.y + m[ 8]*rhs.z + m[12]*rhs.w,
595  m[ 1]*rhs.x + m[ 5]*rhs.y + m[ 9]*rhs.z + m[13]*rhs.w,
596  m[ 2]*rhs.x + m[ 6]*rhs.y + m[10]*rhs.z + m[14]*rhs.w,
597  m[ 3]*rhs.x + m[ 7]*rhs.y + m[11]*rhs.z + m[15]*rhs.w
598  );
599 }
600 
601 template< typename T >
603 {
604  Matrix44<T> ret;
605  for( int i = 0; i < DIM_SQ; ++i ) {
606  ret.m[i] = m[i]*rhs;
607  }
608  return ret;
609 }
610 
611 template< typename T >
613 {
614  Matrix44<T> ret;
615  T s = (T)1/rhs;
616  for( int i = 0; i < DIM_SQ; ++i ) {
617  ret.m[i] = m[i]*s;
618  }
619  return ret;
620 }
621 
622 template< typename T >
624 {
625  Matrix44<T> ret;
626  for( int i = 0; i < DIM_SQ; ++i ) {
627  ret.m[i] = m[i] + rhs;
628  }
629  return ret;
630 }
631 
632 template< typename T >
634 {
635  Matrix44<T> ret;
636  for( int i = 0; i < DIM_SQ; ++i ) {
637  ret.m[i] = m[i] - rhs;
638  }
639  return ret;
640 }
641 
642 template< typename T >
643 T& Matrix44<T>::at( int row, int col )
644 {
645  assert(row >= 0 && row < DIM);
646  assert(col >= 0 && col < DIM);
647  return m[col*DIM + row];
648 }
649 
650 template< typename T >
651 const T& Matrix44<T>::at( int row, int col ) const
652 {
653  assert(row >= 0 && row < DIM);
654  assert(col >= 0 && col < DIM);
655  return m[col*DIM + row];
656 }
657 
658 template< typename T >
659 void Matrix44<T>::set( const T *dt, bool srcIsRowMajor )
660 {
661  if( srcIsRowMajor ) {
662  m[0] = dt[ 0]; m[4] = dt[ 1]; m[ 8] = dt[ 2]; m[12] = dt[ 3];
663  m[1] = dt[ 4]; m[5] = dt[ 5]; m[ 9] = dt[ 6]; m[13] = dt[ 7];
664  m[2] = dt[ 8]; m[6] = dt[ 9]; m[10] = dt[10]; m[14] = dt[11];
665  m[3] = dt[12]; m[7] = dt[13]; m[11] = dt[14]; m[15] = dt[15];
666  }
667  else {
668  std::memcpy( m, dt, MEM_LEN );
669  }
670 }
671 
672 template< typename T >
673 void Matrix44<T>::set( T d0, T d1, T d2, T d3, T d4, T d5, T d6, T d7, T d8, T d9, T d10, T d11, T d12, T d13, T d14, T d15, bool srcIsRowMajor )
674 {
675  if( srcIsRowMajor ) {
676  m[0] = d0; m[4] = d1; m[ 8] = d2; m[12] = d3;
677  m[1] = d4; m[5] = d5; m[ 9] = d6; m[13] = d7;
678  m[2] = d8; m[6] = d9; m[10] = d10; m[14] = d11;
679  m[3] = d12; m[7] = d13; m[11] = d14; m[15] = d15;
680  }
681  else {
682  m[0] = d0; m[4] = d4; m[ 8] = d8; m[12] = d12;
683  m[1] = d1; m[5] = d5; m[ 9] = d9; m[13] = d13;
684  m[2] = d2; m[6] = d6; m[10] = d10; m[14] = d14;
685  m[3] = d3; m[7] = d7; m[11] = d11; m[15] = d15;
686  }
687 }
688 
689 template< typename T >
691 {
692  size_t i = col*DIM;
693  return Vec4<T>(
694  m[i + 0],
695  m[i + 1],
696  m[i + 2],
697  m[i + 3]
698  );
699 }
700 
701 template< typename T >
702 void Matrix44<T>::setColumn( int col, const Vec4<T> &v )
703 {
704  size_t i = col*DIM;
705  m[i + 0] = v.x;
706  m[i + 1] = v.y;
707  m[i + 2] = v.z;
708  m[i + 3] = v.w;
709 }
710 
711 template< typename T >
713 {
714  return Vec4<T>(
715  m[row + 0],
716  m[row + 4],
717  m[row + 8],
718  m[row + 12]
719  );
720 }
721 
722 template< typename T >
723 void Matrix44<T>::setRow( int row, const Vec4<T> &v )
724 {
725  m[row + 0] = v.x;
726  m[row + 4] = v.y;
727  m[row + 8] = v.z;
728  m[row + 12] = v.w;
729 }
730 
731 template< typename T >
732 void Matrix44<T>::getColumns( Vec4<T> *c0, Vec4<T> *c1, Vec4<T> *c2, Vec4<T> *c3 ) const
733 {
734  *c0 = getColumn( 0 );
735  *c1 = getColumn( 1 );
736  *c2 = getColumn( 2 );
737  *c3 = getColumn( 3 );
738 }
739 
740 template< typename T >
741 void Matrix44<T>::setColumns( const Vec4<T> &c0, const Vec4<T> &c1, const Vec4<T> &c2, const Vec4<T> &c3 )
742 {
743  setColumn( 0, c0 );
744  setColumn( 1, c1 );
745  setColumn( 2, c2 );
746  setColumn( 3, c3 );
747 }
748 
749 template< typename T >
750 void Matrix44<T>::getRows( Vec4<T> *r0, Vec4<T> *r1, Vec4<T> *r2, Vec4<T> *r3 ) const
751 {
752  *r0 = getRow( 0 );
753  *r1 = getRow( 1 );
754  *r2 = getRow( 2 );
755  *r3 = getRow( 3 );
756 }
757 
758 template< typename T >
759 void Matrix44<T>::setRows( const Vec4<T> &r0, const Vec4<T> &r1, const Vec4<T> &r2, const Vec4<T> &r3 )
760 {
761  setRow( 0, r0 );
762  setRow( 1, r1 );
763  setRow( 2, r2 );
764  setRow( 3, r3 );
765 }
766 
767 template< typename T >
769 {
770  Matrix22<T> ret;
771  ret.setToNull();
772 
773  for( int i = 0; i < 2; ++i ) {
774  int r = row + i;
775  if( r >= 4 ) {
776  continue;
777  }
778  for( int j = 0; j < 2; ++j ) {
779  int c = col + j;
780  if( c >= 4 ) {
781  continue;
782  }
783  ret.at( i, j ) = at( r, c );
784  }
785  }
786 
787  return ret;
788 }
789 
790 template< typename T >
792 {
793  Matrix33<T> ret;
794  ret.setToNull();
795 
796  for( int i = 0; i < 3; ++i ) {
797  int r = row + i;
798  if( r >= 4 ) {
799  continue;
800  }
801  for( int j = 0; j < 3; ++j ) {
802  int c = col + j;
803  if( c >= 4 ) {
804  continue;
805  }
806  ret.at( i, j ) = at( r, c );
807  }
808  }
809 
810  return ret;
811 }
812 
813 template< typename T >
815 {
816  std::memset( m, 0, MEM_LEN );
817 }
818 
819 template< typename T >
821 {
822  m[ 0] = 1; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
823  m[ 1] = 0; m[ 5] = 1; m[ 9] = 0; m[13] = 0;
824  m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0;
825  m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
826 }
827 
828 template< typename T >
830 {
831  T a0 = m[ 0]*m[ 5] - m[ 1]*m[ 4];
832  T a1 = m[ 0]*m[ 6] - m[ 2]*m[ 4];
833  T a2 = m[ 0]*m[ 7] - m[ 3]*m[ 4];
834  T a3 = m[ 1]*m[ 6] - m[ 2]*m[ 5];
835  T a4 = m[ 1]*m[ 7] - m[ 3]*m[ 5];
836  T a5 = m[ 2]*m[ 7] - m[ 3]*m[ 6];
837  T b0 = m[ 8]*m[13] - m[ 9]*m[12];
838  T b1 = m[ 8]*m[14] - m[10]*m[12];
839  T b2 = m[ 8]*m[15] - m[11]*m[12];
840  T b3 = m[ 9]*m[14] - m[10]*m[13];
841  T b4 = m[ 9]*m[15] - m[11]*m[13];
842  T b5 = m[10]*m[15] - m[11]*m[14];
843 
844  T det = a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
845 
846  return det;
847 }
848 
849 template< typename T >
850 T Matrix44<T>::trace( bool fullTrace ) const
851 {
852  if( fullTrace ) {
853  return m00 + m11 + m22 + m33;
854  }
855 
856  return m00 + m11 + m22;
857 }
858 
859 template< typename T >
861 {
862  Matrix44 ret;
863  ret.m00 = m00; ret.m01 = 0; ret.m02 = 0; ret.m03 = 0;
864  ret.m10 = 0; ret.m11 = m11; ret.m12 = 0; ret.m13 = 0;
865  ret.m20 = 0; ret.m21 = 0; ret.m22 = m22; ret.m23 = 0;
866  ret.m30 = 0; ret.m31 = 0; ret.m32 = 0; ret.m33 = m33;
867  return ret;
868 }
869 
870 template< typename T >
872 {
873  Matrix44 ret;
874  ret.m00 = m00; ret.m01 = 0; ret.m02 = 0; ret.m03 = 0;
875  ret.m10 = m10; ret.m11 = m11; ret.m12 = 0; ret.m13 = 0;
876  ret.m20 = m20; ret.m21 = m21; ret.m22 = m22; ret.m23 = 0;
877  ret.m30 = m30; ret.m31 = m31; ret.m32 = m32; ret.m33 = m33;
878  return ret;
879 }
880 
881 template< typename T >
883 {
884  Matrix44 ret;
885  ret.m00 = m00; ret.m01 = m01; ret.m02 = m02; ret.m03 = m03;
886  ret.m10 = 0; ret.m11 = m11; ret.m12 = m12; ret.m13 = m13;
887  ret.m20 = 0; ret.m21 = 0; ret.m22 = m22; ret.m23 = m23;
888  ret.m30 = 0; ret.m31 = 0; ret.m32 = 0; ret.m33 = m33;
889  return ret;
890 }
891 
892 template< typename T >
894 {
895  T t;
896  t = m01; m01 = m10; m10 = t;
897  t = m02; m02 = m20; m20 = t;
898  t = m03; m03 = m30; m30 = t;
899  t = m12; m12 = m21; m21 = t;
900  t = m13; m13 = m31; m31 = t;
901  t = m23; m23 = m32; m32 = t;
902 }
903 
904 template< typename T >
906 {
907  return Matrix44<T>(
908  m[ 0], m[ 4], m[ 8], m[12],
909  m[ 1], m[ 5], m[ 9], m[13],
910  m[ 2], m[ 6], m[10], m[14],
911  m[ 3], m[ 7], m[11], m[15]
912  );
913 }
914 
915 template< typename T >
916 inline Matrix44<T> Matrix44<T>::inverted( T epsilon ) const
917 {
918  Matrix44<T> inv( (T)0 );
919 
920  T a0 = m[ 0]*m[ 5] - m[ 1]*m[ 4];
921  T a1 = m[ 0]*m[ 6] - m[ 2]*m[ 4];
922  T a2 = m[ 0]*m[ 7] - m[ 3]*m[ 4];
923  T a3 = m[ 1]*m[ 6] - m[ 2]*m[ 5];
924  T a4 = m[ 1]*m[ 7] - m[ 3]*m[ 5];
925  T a5 = m[ 2]*m[ 7] - m[ 3]*m[ 6];
926  T b0 = m[ 8]*m[13] - m[ 9]*m[12];
927  T b1 = m[ 8]*m[14] - m[10]*m[12];
928  T b2 = m[ 8]*m[15] - m[11]*m[12];
929  T b3 = m[ 9]*m[14] - m[10]*m[13];
930  T b4 = m[ 9]*m[15] - m[11]*m[13];
931  T b5 = m[10]*m[15] - m[11]*m[14];
932 
933  T det = a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
934 
935  if( fabs( det ) > epsilon ) {
936  inv.m[ 0] = + m[ 5]*b5 - m[ 6]*b4 + m[ 7]*b3;
937  inv.m[ 4] = - m[ 4]*b5 + m[ 6]*b2 - m[ 7]*b1;
938  inv.m[ 8] = + m[ 4]*b4 - m[ 5]*b2 + m[ 7]*b0;
939  inv.m[12] = - m[ 4]*b3 + m[ 5]*b1 - m[ 6]*b0;
940  inv.m[ 1] = - m[ 1]*b5 + m[ 2]*b4 - m[ 3]*b3;
941  inv.m[ 5] = + m[ 0]*b5 - m[ 2]*b2 + m[ 3]*b1;
942  inv.m[ 9] = - m[ 0]*b4 + m[ 1]*b2 - m[ 3]*b0;
943  inv.m[13] = + m[ 0]*b3 - m[ 1]*b1 + m[ 2]*b0;
944  inv.m[ 2] = + m[13]*a5 - m[14]*a4 + m[15]*a3;
945  inv.m[ 6] = - m[12]*a5 + m[14]*a2 - m[15]*a1;
946  inv.m[10] = + m[12]*a4 - m[13]*a2 + m[15]*a0;
947  inv.m[14] = - m[12]*a3 + m[13]*a1 - m[14]*a0;
948  inv.m[ 3] = - m[ 9]*a5 + m[10]*a4 - m[11]*a3;
949  inv.m[ 7] = + m[ 8]*a5 - m[10]*a2 + m[11]*a1;
950  inv.m[11] = - m[ 8]*a4 + m[ 9]*a2 - m[11]*a0;
951  inv.m[15] = + m[ 8]*a3 - m[ 9]*a1 + m[10]*a0;
952 
953  T invDet = ((T)1)/det;
954  inv.m[ 0] *= invDet;
955  inv.m[ 1] *= invDet;
956  inv.m[ 2] *= invDet;
957  inv.m[ 3] *= invDet;
958  inv.m[ 4] *= invDet;
959  inv.m[ 5] *= invDet;
960  inv.m[ 6] *= invDet;
961  inv.m[ 7] *= invDet;
962  inv.m[ 8] *= invDet;
963  inv.m[ 9] *= invDet;
964  inv.m[10] *= invDet;
965  inv.m[11] *= invDet;
966  inv.m[12] *= invDet;
967  inv.m[13] *= invDet;
968  inv.m[14] *= invDet;
969  inv.m[15] *= invDet;
970  }
971 
972  return inv;
973 }
974 
975 template< typename T >
977 {
978  return Vec3<T>(
979  v.x*m00 + v.y*m10 + v.z*m20,
980  v.x*m01 + v.y*m11 + v.z*m21,
981  v.x*m02 + v.y*m12 + v.z*m22
982  );
983 }
984 
985 template< typename T >
987 {
988  return Vec4<T>(
989  v.x*m00 + v.y*m10 + v.z*m20 + v.w*m30,
990  v.x*m01 + v.y*m11 + v.z*m21 + v.w*m31,
991  v.x*m02 + v.y*m12 + v.z*m22 + v.w*m32,
992  v.x*m03 + v.y*m13 + v.z*m23 + v.w*m33
993  );
994 }
995 
996 template< typename T >
998 {
999  return Vec3<T>(
1000  m00*v.x + m01*v.y + m02*v.z,
1001  m10*v.x + m11*v.y + m12*v.z,
1002  m20*v.x + m21*v.y + m22*v.z
1003  );
1004 }
1005 
1006 template< typename T >
1008 {
1009  return Vec4<T>(
1010  m00*v.x + m01*v.y + m02*v.z + m03*v.w,
1011  m10*v.x + m11*v.y + m12*v.z + m13*v.w,
1012  m20*v.x + m21*v.y + m22*v.z + m23*v.w,
1013  m30*v.x + m31*v.y + m32*v.z + m33*v.w
1014  );
1015 }
1016 
1017 template< typename T >
1019 {
1020  Matrix44<T> ret;
1021 
1022  // compute upper left 3x3 matrix determinant
1023  T cofactor0 = m[ 5]*m[10] - m[6]*m[ 9];
1024  T cofactor4 = m[ 2]*m[ 9] - m[1]*m[10];
1025  T cofactor8 = m[ 1]*m[ 6] - m[2]*m[ 5];
1026  T det = m[0]*cofactor0 + m[4]*cofactor4 + m[8]*cofactor8;
1027  if( fabs( det ) <= EPSILON ) {
1028  return ret;
1029  }
1030 
1031  // create adjunct matrix and multiply by 1/det to get upper 3x3
1032  T invDet = ((T)1.0) / det;
1033  ret.m[ 0] = invDet*cofactor0;
1034  ret.m[ 1] = invDet*cofactor4;
1035  ret.m[ 2] = invDet*cofactor8;
1036 
1037  ret.m[ 4] = invDet*(m[ 6]*m[ 8] - m[ 4]*m[10]);
1038  ret.m[ 5] = invDet*(m[ 0]*m[10] - m[ 2]*m[ 8]);
1039  ret.m[ 6] = invDet*(m[ 2]*m[ 4] - m[ 0]*m[ 6]);
1040 
1041  ret.m[ 8] = invDet*(m[ 4]*m[ 9] - m[ 5]*m[ 8]);
1042  ret.m[ 9] = invDet*(m[ 1]*m[ 8] - m[ 0]*m[ 9]);
1043  ret.m[10] = invDet*(m[ 0]*m[ 5] - m[ 1]*m[ 4]);
1044 
1045  // multiply -translation by inverted 3x3 to get its inverse
1046  ret.m[12] = -ret.m[ 0]*m[12] - ret.m[ 4]*m[13] - ret.m[ 8]*m[14];
1047  ret.m[13] = -ret.m[ 1]*m[12] - ret.m[ 5]*m[13] - ret.m[ 9]*m[14];
1048  ret.m[14] = -ret.m[ 2]*m[12] - ret.m[ 6]*m[13] - ret.m[10]*m[14];
1049 
1050  return ret;
1051 }
1052 
1053 template< typename T >
1055 {
1056  T x = m00*rhs.x + m01*rhs.y + m02*rhs.z + m03;
1057  T y = m10*rhs.x + m11*rhs.y + m12*rhs.z + m13;
1058  T z = m20*rhs.x + m21*rhs.y + m22*rhs.z + m23;
1059  T w = m30*rhs.x + m31*rhs.y + m32*rhs.z + m33;
1060 
1061  return Vec3<T>( x / w, y / w, z / w );
1062 }
1063 
1064 template< typename T >
1066 {
1067  T x = m00*rhs.x + m01*rhs.y + m02*rhs.z + m03;
1068  T y = m10*rhs.x + m11*rhs.y + m12*rhs.z + m13;
1069  T z = m20*rhs.x + m21*rhs.y + m22*rhs.z + m23;
1070 
1071  return Vec3<T>( x, y, z );
1072 }
1073 
1074 template< typename T >
1076 {
1077  T x = m00*rhs.x + m01*rhs.y + m02*rhs.z;
1078  T y = m10*rhs.x + m11*rhs.y + m12*rhs.z;
1079  T z = m20*rhs.x + m21*rhs.y + m22*rhs.z;
1080 
1081  return Vec3<T>( x, y, z );
1082 }
1083 
1084 template< typename T > // thanks to @juj/MathGeoLib for fix
1086 {
1087  // transpose upper 3x3 (R->R^t)
1088  std::swap( at(0,1), at(1,0) );
1089  std::swap( at(0,2), at(2,0) );
1090  std::swap( at(1,2), at(2,1) );
1091 
1092  // replace translation (T) with R^t(-T).
1093  Vec3f newT( transformVec( Vec3f(-at(0,3),-at(1,3),-at(2,3)) ) );
1094  at(0,3) = newT.x;
1095  at(1,3) = newT.y;
1096  at(2,3) = newT.z;
1097 }
1098 
1099 template<typename T>
1101 {
1102  Matrix44 ret;
1103  ret.m[12] = v.x;
1104  ret.m[13] = v.y;
1105  ret.m[14] = v.z;
1106  ret.m[15] = w;
1107 
1108  return ret;
1109 }
1110 
1112 // Matrix44
1113 template<typename T>
1114 Matrix44<T> Matrix44<T>::createRotation( const Vec3<T> &from, const Vec3<T> &to, const Vec3<T> &worldUp )
1115 {
1116  // The goal is to obtain a rotation matrix that takes
1117  // "fromDir" to "toDir". We do this in two steps and
1118  // compose the resulting rotation matrices;
1119  // (a) rotate "fromDir" into the z-axis
1120  // (b) rotate the z-axis into "toDir"
1121 
1122  // The from direction must be non-zero; but we allow zero to and up dirs.
1123  if( from.lengthSquared() == 0 ) {
1124  return Matrix44<T>();
1125  }
1126  else {
1127  Matrix44<T> zAxis2FromDir = alignZAxisWithTarget( from, Vec3<T>::yAxis() );
1128  Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed();
1129  Matrix44<T> zAxis2ToDir = alignZAxisWithTarget( to, worldUp );
1130  return fromDir2zAxis * zAxis2ToDir;
1131  }
1132 }
1133 
1134 template<typename T>
1136 {
1137  Vec3<T> unit( axis.normalized() );
1138  T sine = math<T>::sin( angle );
1139  T cosine = math<T>::cos( angle );
1140 
1141  Matrix44<T> ret;
1142 
1143  ret.m[ 0] = unit.x * unit.x * (1 - cosine) + cosine;
1144  ret.m[ 1] = unit.x * unit.y * (1 - cosine) + unit.z * sine;
1145  ret.m[ 2] = unit.x * unit.z * (1 - cosine) - unit.y * sine;
1146  ret.m[ 3] = 0;
1147 
1148  ret.m[ 4] = unit.x * unit.y * (1 - cosine) - unit.z * sine;
1149  ret.m[ 5] = unit.y * unit.y * (1 - cosine) + cosine;
1150  ret.m[ 6] = unit.y * unit.z * (1 - cosine) + unit.x * sine;
1151  ret.m[ 7] = 0;
1152 
1153  ret.m[ 8] = unit.x * unit.z * (1 - cosine) + unit.y * sine;
1154  ret.m[ 9] = unit.y * unit.z * (1 - cosine) - unit.x * sine;
1155  ret.m[10] = unit.z * unit.z * (1 - cosine) + cosine;
1156  ret.m[11] = 0;
1157 
1158  ret.m[12] = 0;
1159  ret.m[13] = 0;
1160  ret.m[14] = 0;
1161  ret.m[15] = 1;
1162 
1163  return ret;
1164 }
1165 
1166 template<typename T>
1168 {
1169  // The ordering for this is XYZ. In OpenGL, the ordering
1170  // is the same, but the operations needs to happen in
1171  // reverse:
1172  //
1173  // glRotatef( eulerRadians.z, 0.0f, 0.0f 1.0f );
1174  // glRotatef( eulerRadians.y, 0.0f, 1.0f 0.0f );
1175  // glRotatef( eulerRadians.x, 1.0f, 0.0f 0.0f );
1176  //
1177 
1178  Matrix44<T> ret;
1179  T cos_rz, sin_rz, cos_ry, sin_ry, cos_rx, sin_rx;
1180 
1181  cos_rx = math<T>::cos( eulerRadians.x );
1182  cos_ry = math<T>::cos( eulerRadians.y );
1183  cos_rz = math<T>::cos( eulerRadians.z );
1184 
1185  sin_rx = math<T>::sin( eulerRadians.x );
1186  sin_ry = math<T>::sin( eulerRadians.y );
1187  sin_rz = math<T>::sin( eulerRadians.z );
1188 
1189  ret.m[ 0] = cos_rz*cos_ry;
1190  ret.m[ 1] = sin_rz*cos_ry;
1191  ret.m[ 2] = -sin_ry;
1192  ret.m[ 3] = 0;
1193 
1194  ret.m[ 4] = -sin_rz*cos_rx + cos_rz*sin_ry*sin_rx;
1195  ret.m[ 5] = cos_rz*cos_rx + sin_rz*sin_ry*sin_rx;
1196  ret.m[ 6] = cos_ry*sin_rx;
1197  ret.m[ 7] = 0;
1198 
1199  ret.m[ 8] = sin_rz*sin_rx + cos_rz*sin_ry*cos_rx;
1200  ret.m[ 9] = -cos_rz*sin_rx + sin_rz*sin_ry*cos_rx;
1201  ret.m[10] = cos_ry*cos_rx;
1202  ret.m[11] = 0;
1203 
1204  ret.m[12] = 0;
1205  ret.m[13] = 0;
1206  ret.m[14] = 0;
1207  ret.m[15] = 1;
1208 
1209  return ret;
1210 }
1211 
1212 template<typename T>
1214 {
1215  return Matrix44<T>(
1216  u.x, u.y, u.z, 0,
1217  v.x, v.y, v.z, 0,
1218  w.x, w.y, w.z, 0,
1219  0, 0, 0, 1
1220  );
1221 }
1222 
1223 template<typename T>
1225 {
1226  Matrix44<T> ret;
1227  ret.setToIdentity();
1228  ret.at(0,0) = s;
1229  ret.at(1,1) = s;
1230  ret.at(2,2) = s;
1231  ret.at(3,3) = s;
1232  return ret;
1233 }
1234 
1235 template<typename T>
1237 {
1238  Matrix44<T> ret;
1239  ret.setToIdentity();
1240  ret.at(0,0) = v.x;
1241  ret.at(1,1) = v.y;
1242  ret.at(2,2) = 1;
1243  ret.at(3,3) = 1;
1244  return ret;
1245 }
1246 
1247 template<typename T>
1249 {
1250  Matrix44<T> ret;
1251  ret.setToIdentity();
1252  ret.at(0,0) = v.x;
1253  ret.at(1,1) = v.y;
1254  ret.at(2,2) = v.z;
1255  ret.at(3,3) = 1;
1256  return ret;
1257 }
1258 
1259 template<typename T>
1261 {
1262  Matrix44<T> ret;
1263  ret.setToIdentity();
1264  ret.at(0,0) = v.x;
1265  ret.at(1,1) = v.y;
1266  ret.at(2,2) = v.z;
1267  ret.at(3,3) = v.w;
1268  return ret;
1269 }
1270 
1271 template<typename T>
1273 {
1274  // Ensure that the target direction is non-zero.
1275  if( targetDir.lengthSquared() == 0 )
1276  targetDir = Vec3<T>::zAxis();
1277 
1278  // Ensure that the up direction is non-zero.
1279  if( upDir.lengthSquared() == 0 )
1280  upDir = Vec3<T>::yAxis();
1281 
1282  // Check for degeneracies. If the upDir and targetDir are parallel
1283  // or opposite, then compute a new, arbitrary up direction that is
1284  // not parallel or opposite to the targetDir.
1285  if( upDir.cross( targetDir ).lengthSquared() == 0 ) {
1286  upDir = targetDir.cross( Vec3<T>::xAxis() );
1287  if( upDir.lengthSquared() == 0 )
1288  upDir = targetDir.cross( Vec3<T>::zAxis() );
1289  }
1290 
1291  // Compute the x-, y-, and z-axis vectors of the new coordinate system.
1292  Vec3<T> targetPerpDir = upDir.cross( targetDir );
1293  Vec3<T> targetUpDir = targetDir.cross( targetPerpDir );
1294 
1295 
1296  // Rotate the x-axis into targetPerpDir (row 0),
1297  // rotate the y-axis into targetUpDir (row 1),
1298  // rotate the z-axis into targetDir (row 2).
1299  Vec3<T> row[3];
1300  row[0] = targetPerpDir.normalized();
1301  row[1] = targetUpDir.normalized();
1302  row[2] = targetDir.normalized();
1303 
1304  Matrix44<T> mat( row[0].x, row[0].y, row[0].z, 0,
1305  row[1].x, row[1].y, row[1].z, 0,
1306  row[2].x, row[2].y, row[2].z, 0,
1307  0, 0, 0, 1 );
1308 
1309  return mat;
1310 }
1311 
1313 // Typedefs
1316 
1317 } // namespace cinder
GLdouble GLdouble GLdouble r
Definition: GLee.h:1474
T x
Definition: Vector.h:694
Matrix44< T > & operator=(const Matrix44< T > &rhs)
Definition: Matrix44.h:378
void rotate(const Vec3< T > &from, const Vec3< T > &to, const Vec3< T > &worldUp)
Definition: Matrix44.h:231
T m22
Definition: Matrix33.h:67
GLenum GLint GLint y
Definition: GLee.h:987
T lengthSquared() const
Definition: Vector.h:443
Definition: CinderMath.h:40
static Matrix44< T > createRotation(const Vec4< T > &from, const Vec4< T > &to, const Vec4< T > &worldUp)
Definition: Matrix44.h:258
Matrix44< T > & operator/=(T rhs)
Definition: Matrix44.h:504
Matrix44< T > lowerTriangular() const
Definition: Matrix44.h:871
static T cos(T x)
Definition: CinderMath.h:46
Matrix33< T > subMatrix33(int row, int col) const
Definition: Matrix44.h:791
void rotate(const Vec4< T > &eulerRadians)
Definition: Matrix44.h:229
void rotate(const Vec4< T > &axis, T radians)
Definition: Matrix44.h:226
T value_type
Definition: Matrix44.h:45
Matrix44< T > upperTriangular() const
Definition: Matrix44.h:882
void setTranslate(const Vec3< T > &v)
Definition: Matrix44.h:217
void getColumns(Vec4< T > *c0, Vec4< T > *c1, Vec4< T > *c2, Vec4< T > *c3) const
Definition: Matrix44.h:732
T m30
Definition: Matrix44.h:71
void scale(T s)
Definition: Matrix44.h:235
static Matrix44< T > createRotationOnb(const Vec4< T > &u, const Vec4< T > &v, const Vec4< T > &w)
Definition: Matrix44.h:264
Vec3< T > transformPoint(const Vec3< T > &rhs) const
Definition: Matrix44.h:1054
void scale(const Vec4< T > &v)
Definition: Matrix44.h:238
T m11
Definition: Matrix44.h:72
static T sin(T x)
Definition: CinderMath.h:47
static Matrix44< T > createRotation(const Vec4< T > &axis, T radians)
Definition: Matrix44.h:256
Matrix44< T > invertTransform() const
bool equalCompare(const Matrix44< T > &rhs, T epsilon) const
Definition: Matrix44.h:435
T & at(int row, int col)
Definition: Matrix33.h:501
T y
Definition: Vector.h:694
const Matrix44< T > operator+(const Matrix44< T > &rhs) const
Definition: Matrix44.h:560
T m01
Definition: Matrix22.h:64
Matrix44()
Definition: Matrix44.h:290
T mcols[4][4]
Definition: Matrix44.h:77
#define EPSILON
Definition: CinderMath.h:125
T z
Definition: Vector.h:694
Vec4< T > getColumn(int col) const
Definition: Matrix44.h:690
T m31
Definition: Matrix44.h:72
T z
Definition: Vector.h:321
T w
Definition: Vector.h:694
static Matrix44< T > createTranslation(const Vec3< T > &v, T w=1)
Definition: Matrix44.h:1100
void setToNull()
Definition: Matrix22.h:553
GLuint src
Definition: GLee.h:10873
T x
Definition: Vector.h:321
void scale(const Vec3< T > &v)
Definition: Matrix44.h:237
Definition: Vector.h:691
Matrix44< T > diagonal() const
Definition: Matrix44.h:860
bool operator==(const Matrix44< T > &rhs) const
Definition: Matrix44.h:118
Matrix44< T > orthonormalInverted() const
Definition: Matrix44.h:202
T x
Definition: Vector.h:71
GLfloat angle
Definition: GLee.h:13523
void transpose()
Definition: Matrix44.h:893
T y
Definition: Vector.h:321
T m32
Definition: Matrix44.h:73
void rotate(const Vec4< T > &from, const Vec4< T > &to, const Vec4< T > &worldUp)
Definition: Matrix44.h:232
static const size_t DIM_SQ
Definition: Matrix44.h:48
T m[16]
Definition: Matrix44.h:66
void setTranslate(const Vec4< T > &v)
Definition: Matrix44.h:218
void getRows(Vec4< T > *r0, Vec4< T > *r1, Vec4< T > *r2, Vec4< T > *r3) const
Definition: Matrix44.h:750
void rotate(const Vec3< T > &axis, T radians)
Definition: Matrix44.h:225
T trace(bool fullTrace=false) const
Definition: Matrix44.h:850
static Matrix44< T > alignZAxisWithTarget(Vec3< T > targetDir, Vec3< T > upDir)
Definition: Matrix44.h:1272
void setColumn(int col, const Vec4< T > &v)
Definition: Matrix44.h:702
T m12
Definition: Matrix33.h:67
static Matrix44< T > zero()
Definition: Matrix44.h:248
static const size_t MEM_LEN
Definition: Matrix44.h:49
void scale(const Vec2< T > &v)
Definition: Matrix44.h:236
T m20
Definition: Matrix44.h:71
static Matrix44< T > alignZAxisWithTarget(Vec4< T > targetDir, Vec4< T > upDir)
Definition: Matrix44.h:274
Definition: Vector.h:68
Matrix44< T > & operator*=(const Matrix44< T > &rhs)
Definition: Matrix44.h:445
T m10
Definition: Matrix22.h:63
Matrix44< T > & operator+=(const Matrix44< T > &rhs)
Definition: Matrix44.h:477
Vec4< T > getRow(int row) const
Definition: Matrix44.h:712
T m20
Definition: Matrix33.h:65
GLenum GLenum GLvoid * row
Definition: GLee.h:1089
Vec3< T > preMultiply(const Vec3< T > &v) const
Definition: Matrix44.h:976
Matrix22< T > subMatrix22(int row, int col) const
Definition: Matrix44.h:768
Vec3< float > Vec3f
Definition: Vector.h:1317
T m11
Definition: Matrix33.h:66
Definition: Vector.h:65
void setRow(int row, const Vec4< T > &v)
Definition: Matrix44.h:723
T m00
Definition: Matrix33.h:65
const Matrix44< T > operator/(T rhs) const
Definition: Matrix44.h:612
T m10
Definition: Matrix44.h:71
T m21
Definition: Matrix33.h:66
T m10
Definition: Matrix33.h:65
Vec3< T > postMultiply(const Vec3< T > &v) const
Definition: Matrix44.h:997
Vec3< T > transformVec(const Vec3< T > &rhs) const
Definition: Matrix44.h:1075
Vec4< T > getTranslate() const
Definition: Matrix44.h:215
void rotate(const Vec3< T > &eulerRadians)
Definition: Matrix44.h:228
GLenum GLint x
Definition: GLee.h:987
T m23
Definition: Matrix44.h:74
Matrix44< T > affineInverted() const
Definition: Matrix44.h:1018
T & at(int row, int col)
Definition: Matrix44.h:643
T m01
Definition: Matrix33.h:66
static const size_t DIM
Definition: Matrix44.h:47
T & at(int row, int col)
Definition: Matrix22.h:450
void setToNull()
Definition: Matrix44.h:814
Vec3< T > xyz() const
Definition: Vector.h:973
const GLdouble * v
Definition: GLee.h:1384
GLdouble GLdouble z
Definition: GLee.h:1911
T y
Definition: Vector.h:71
static Matrix44< T > createRotationOnb(const Vec3< T > &u, const Vec3< T > &v, const Vec3< T > &w)
Definition: Matrix44.h:1213
const Matrix44< T > operator-(const Matrix44< T > &rhs) const
Definition: Matrix44.h:570
T m13
Definition: Matrix44.h:74
const GLubyte * c
Definition: GLee.h:8491
Matrix44< float > Matrix44f
Definition: Matrix44.h:1314
void setRows(const Vec4< T > &r0, const Vec4< T > &r1, const Vec4< T > &r2, const Vec4< T > &r3)
Definition: Matrix44.h:759
bool operator!=(const Matrix44< T > &rhs) const
Definition: Matrix44.h:119
Represents a two dimensional affine transformation.
Definition: MatrixAffine2.h:38
const Matrix44< T > operator*(const Matrix44< T > &rhs) const
Definition: Matrix44.h:532
Matrix44< T > inverted(T epsilon=FLT_MIN) const
Definition: Matrix44.h:916
Definition: Matrix44.h:41
void affineInvert()
Computes inverse; assumes the matrix is affine, i.e. the bottom row is [0 0 0 1]. ...
Definition: Matrix44.h:197
T TYPE
Definition: Matrix44.h:44
Vec3< T > cross(const Vec3< T > &rhs) const
Definition: Vector.h:423
T m00
Definition: Matrix44.h:71
T m00
Definition: Matrix22.h:63
void setToNull()
Definition: Matrix33.h:638
void translate(const Vec3< T > &tr)
Definition: Matrix44.h:221
void orthonormalInvert()
Computes inverse; assumes the matrix is orthonormal.
Definition: Matrix44.h:1085
T m03
Definition: Matrix44.h:74
Definition: Matrix22.h:37
void invert(T epsilon=FLT_MIN)
Definition: Matrix44.h:185
Matrix44< T > transposed() const
Definition: Matrix44.h:905
T m21
Definition: Matrix44.h:72
T m33
Definition: Matrix44.h:74
static Matrix44< T > identity()
Definition: Matrix44.h:244
Vec4< T > transformVec(const Vec4< T > &rhs) const
Definition: Matrix44.h:212
void set(const T *dt, bool srcIsRowMajor=false)
Definition: Matrix44.h:659
T m[6]
Definition: MatrixAffine2.h:61
T m11
Definition: Matrix22.h:64
GLubyte GLubyte GLubyte GLubyte w
Definition: GLee.h:2685
static Matrix44< T > one()
Definition: Matrix44.h:246
static Matrix44< T > createScale(T s)
Definition: Matrix44.h:1224
static Matrix44< T > createRotation(const Vec4< T > &eulerRadians)
Definition: Matrix44.h:261
Vec3< T > transformPointAffine(const Vec3< T > &rhs) const
Definition: Matrix44.h:1065
T m02
Definition: Matrix33.h:67
T determinant() const
Definition: Matrix44.h:829
const GLfloat * m
Definition: GLee.h:13493
Matrix44< T > & operator-=(const Matrix44< T > &rhs)
Definition: Matrix44.h:486
GLdouble GLdouble t
Definition: GLee.h:1426
GLdouble s
Definition: GLee.h:1378
static Matrix44< T > createRotation(const Vec3< T > &axis, T radians)
Definition: Matrix44.h:1135
T m02
Definition: Matrix44.h:73
void setToIdentity()
Definition: Matrix44.h:820
Matrix44< double > Matrix44d
Definition: Matrix44.h:1315
Definition: Matrix33.h:37
static Matrix44< T > createTranslation(const Vec4< T > &v)
Definition: Matrix44.h:252
void translate(const Vec4< T > &tr)
Definition: Matrix44.h:222
T m01
Definition: Matrix44.h:72
void setColumns(const Vec4< T > &c0, const Vec4< T > &c1, const Vec4< T > &c2, const Vec4< T > &c3)
Definition: Matrix44.h:741
Vec3< T > normalized() const
Definition: Vector.h:492
T m22
Definition: Matrix44.h:73
T m12
Definition: Matrix44.h:73