1 /*
2 Copyright (C) 2009 Sony Computer Entertainment Inc.
3 All rights reserved.
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14
15 */
16
17 #ifndef _VECTORMATH_MAT_AOS_CPP_H
18 #define _VECTORMATH_MAT_AOS_CPP_H
19
20 namespace Vectormath {
21 namespace Aos {
22
23 //-----------------------------------------------------------------------------
24 // Constants
25
26 #define _VECTORMATH_PI_OVER_2 1.570796327f
27
28 //-----------------------------------------------------------------------------
29 // Definitions
30
Matrix3(const Matrix3 & mat)31 inline Matrix3::Matrix3( const Matrix3 & mat )
32 {
33 mCol0 = mat.mCol0;
34 mCol1 = mat.mCol1;
35 mCol2 = mat.mCol2;
36 }
37
Matrix3(float scalar)38 inline Matrix3::Matrix3( float scalar )
39 {
40 mCol0 = Vector3( scalar );
41 mCol1 = Vector3( scalar );
42 mCol2 = Vector3( scalar );
43 }
44
Matrix3(const Quat & unitQuat)45 inline Matrix3::Matrix3( const Quat & unitQuat )
46 {
47 float qx, qy, qz, qw, qx2, qy2, qz2, qxqx2, qyqy2, qzqz2, qxqy2, qyqz2, qzqw2, qxqz2, qyqw2, qxqw2;
48 qx = unitQuat.getX();
49 qy = unitQuat.getY();
50 qz = unitQuat.getZ();
51 qw = unitQuat.getW();
52 qx2 = ( qx + qx );
53 qy2 = ( qy + qy );
54 qz2 = ( qz + qz );
55 qxqx2 = ( qx * qx2 );
56 qxqy2 = ( qx * qy2 );
57 qxqz2 = ( qx * qz2 );
58 qxqw2 = ( qw * qx2 );
59 qyqy2 = ( qy * qy2 );
60 qyqz2 = ( qy * qz2 );
61 qyqw2 = ( qw * qy2 );
62 qzqz2 = ( qz * qz2 );
63 qzqw2 = ( qw * qz2 );
64 mCol0 = Vector3( ( ( 1.0f - qyqy2 ) - qzqz2 ), ( qxqy2 + qzqw2 ), ( qxqz2 - qyqw2 ) );
65 mCol1 = Vector3( ( qxqy2 - qzqw2 ), ( ( 1.0f - qxqx2 ) - qzqz2 ), ( qyqz2 + qxqw2 ) );
66 mCol2 = Vector3( ( qxqz2 + qyqw2 ), ( qyqz2 - qxqw2 ), ( ( 1.0f - qxqx2 ) - qyqy2 ) );
67 }
68
Matrix3(const Vector3 & _col0,const Vector3 & _col1,const Vector3 & _col2)69 inline Matrix3::Matrix3( const Vector3 & _col0, const Vector3 & _col1, const Vector3 & _col2 )
70 {
71 mCol0 = _col0;
72 mCol1 = _col1;
73 mCol2 = _col2;
74 }
75
setCol0(const Vector3 & _col0)76 inline Matrix3 & Matrix3::setCol0( const Vector3 & _col0 )
77 {
78 mCol0 = _col0;
79 return *this;
80 }
81
setCol1(const Vector3 & _col1)82 inline Matrix3 & Matrix3::setCol1( const Vector3 & _col1 )
83 {
84 mCol1 = _col1;
85 return *this;
86 }
87
setCol2(const Vector3 & _col2)88 inline Matrix3 & Matrix3::setCol2( const Vector3 & _col2 )
89 {
90 mCol2 = _col2;
91 return *this;
92 }
93
setCol(int col,const Vector3 & vec)94 inline Matrix3 & Matrix3::setCol( int col, const Vector3 & vec )
95 {
96 *(&mCol0 + col) = vec;
97 return *this;
98 }
99
setRow(int row,const Vector3 & vec)100 inline Matrix3 & Matrix3::setRow( int row, const Vector3 & vec )
101 {
102 mCol0.setElem( row, vec.getElem( 0 ) );
103 mCol1.setElem( row, vec.getElem( 1 ) );
104 mCol2.setElem( row, vec.getElem( 2 ) );
105 return *this;
106 }
107
setElem(int col,int row,float val)108 inline Matrix3 & Matrix3::setElem( int col, int row, float val )
109 {
110 Vector3 tmpV3_0;
111 tmpV3_0 = this->getCol( col );
112 tmpV3_0.setElem( row, val );
113 this->setCol( col, tmpV3_0 );
114 return *this;
115 }
116
getElem(int col,int row)117 inline float Matrix3::getElem( int col, int row ) const
118 {
119 return this->getCol( col ).getElem( row );
120 }
121
getCol0()122 inline const Vector3 Matrix3::getCol0( ) const
123 {
124 return mCol0;
125 }
126
getCol1()127 inline const Vector3 Matrix3::getCol1( ) const
128 {
129 return mCol1;
130 }
131
getCol2()132 inline const Vector3 Matrix3::getCol2( ) const
133 {
134 return mCol2;
135 }
136
getCol(int col)137 inline const Vector3 Matrix3::getCol( int col ) const
138 {
139 return *(&mCol0 + col);
140 }
141
getRow(int row)142 inline const Vector3 Matrix3::getRow( int row ) const
143 {
144 return Vector3( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ) );
145 }
146
147 inline Vector3 & Matrix3::operator []( int col )
148 {
149 return *(&mCol0 + col);
150 }
151
152 inline const Vector3 Matrix3::operator []( int col ) const
153 {
154 return *(&mCol0 + col);
155 }
156
157 inline Matrix3 & Matrix3::operator =( const Matrix3 & mat )
158 {
159 mCol0 = mat.mCol0;
160 mCol1 = mat.mCol1;
161 mCol2 = mat.mCol2;
162 return *this;
163 }
164
transpose(const Matrix3 & mat)165 inline const Matrix3 transpose( const Matrix3 & mat )
166 {
167 return Matrix3(
168 Vector3( mat.getCol0().getX(), mat.getCol1().getX(), mat.getCol2().getX() ),
169 Vector3( mat.getCol0().getY(), mat.getCol1().getY(), mat.getCol2().getY() ),
170 Vector3( mat.getCol0().getZ(), mat.getCol1().getZ(), mat.getCol2().getZ() )
171 );
172 }
173
inverse(const Matrix3 & mat)174 inline const Matrix3 inverse( const Matrix3 & mat )
175 {
176 Vector3 tmp0, tmp1, tmp2;
177 float detinv;
178 tmp0 = cross( mat.getCol1(), mat.getCol2() );
179 tmp1 = cross( mat.getCol2(), mat.getCol0() );
180 tmp2 = cross( mat.getCol0(), mat.getCol1() );
181 detinv = ( 1.0f / dot( mat.getCol2(), tmp2 ) );
182 return Matrix3(
183 Vector3( ( tmp0.getX() * detinv ), ( tmp1.getX() * detinv ), ( tmp2.getX() * detinv ) ),
184 Vector3( ( tmp0.getY() * detinv ), ( tmp1.getY() * detinv ), ( tmp2.getY() * detinv ) ),
185 Vector3( ( tmp0.getZ() * detinv ), ( tmp1.getZ() * detinv ), ( tmp2.getZ() * detinv ) )
186 );
187 }
188
determinant(const Matrix3 & mat)189 inline float determinant( const Matrix3 & mat )
190 {
191 return dot( mat.getCol2(), cross( mat.getCol0(), mat.getCol1() ) );
192 }
193
194 inline const Matrix3 Matrix3::operator +( const Matrix3 & mat ) const
195 {
196 return Matrix3(
197 ( mCol0 + mat.mCol0 ),
198 ( mCol1 + mat.mCol1 ),
199 ( mCol2 + mat.mCol2 )
200 );
201 }
202
203 inline const Matrix3 Matrix3::operator -( const Matrix3 & mat ) const
204 {
205 return Matrix3(
206 ( mCol0 - mat.mCol0 ),
207 ( mCol1 - mat.mCol1 ),
208 ( mCol2 - mat.mCol2 )
209 );
210 }
211
212 inline Matrix3 & Matrix3::operator +=( const Matrix3 & mat )
213 {
214 *this = *this + mat;
215 return *this;
216 }
217
218 inline Matrix3 & Matrix3::operator -=( const Matrix3 & mat )
219 {
220 *this = *this - mat;
221 return *this;
222 }
223
224 inline const Matrix3 Matrix3::operator -( ) const
225 {
226 return Matrix3(
227 ( -mCol0 ),
228 ( -mCol1 ),
229 ( -mCol2 )
230 );
231 }
232
absPerElem(const Matrix3 & mat)233 inline const Matrix3 absPerElem( const Matrix3 & mat )
234 {
235 return Matrix3(
236 absPerElem( mat.getCol0() ),
237 absPerElem( mat.getCol1() ),
238 absPerElem( mat.getCol2() )
239 );
240 }
241
242 inline const Matrix3 Matrix3::operator *( float scalar ) const
243 {
244 return Matrix3(
245 ( mCol0 * scalar ),
246 ( mCol1 * scalar ),
247 ( mCol2 * scalar )
248 );
249 }
250
251 inline Matrix3 & Matrix3::operator *=( float scalar )
252 {
253 *this = *this * scalar;
254 return *this;
255 }
256
257 inline const Matrix3 operator *( float scalar, const Matrix3 & mat )
258 {
259 return mat * scalar;
260 }
261
262 inline const Vector3 Matrix3::operator *( const Vector3 & vec ) const
263 {
264 return Vector3(
265 ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
266 ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
267 ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) )
268 );
269 }
270
271 inline const Matrix3 Matrix3::operator *( const Matrix3 & mat ) const
272 {
273 return Matrix3(
274 ( *this * mat.mCol0 ),
275 ( *this * mat.mCol1 ),
276 ( *this * mat.mCol2 )
277 );
278 }
279
280 inline Matrix3 & Matrix3::operator *=( const Matrix3 & mat )
281 {
282 *this = *this * mat;
283 return *this;
284 }
285
mulPerElem(const Matrix3 & mat0,const Matrix3 & mat1)286 inline const Matrix3 mulPerElem( const Matrix3 & mat0, const Matrix3 & mat1 )
287 {
288 return Matrix3(
289 mulPerElem( mat0.getCol0(), mat1.getCol0() ),
290 mulPerElem( mat0.getCol1(), mat1.getCol1() ),
291 mulPerElem( mat0.getCol2(), mat1.getCol2() )
292 );
293 }
294
identity()295 inline const Matrix3 Matrix3::identity( )
296 {
297 return Matrix3(
298 Vector3::xAxis( ),
299 Vector3::yAxis( ),
300 Vector3::zAxis( )
301 );
302 }
303
rotationX(float radians)304 inline const Matrix3 Matrix3::rotationX( float radians )
305 {
306 float s, c;
307 s = sinf( radians );
308 c = cosf( radians );
309 return Matrix3(
310 Vector3::xAxis( ),
311 Vector3( 0.0f, c, s ),
312 Vector3( 0.0f, -s, c )
313 );
314 }
315
rotationY(float radians)316 inline const Matrix3 Matrix3::rotationY( float radians )
317 {
318 float s, c;
319 s = sinf( radians );
320 c = cosf( radians );
321 return Matrix3(
322 Vector3( c, 0.0f, -s ),
323 Vector3::yAxis( ),
324 Vector3( s, 0.0f, c )
325 );
326 }
327
rotationZ(float radians)328 inline const Matrix3 Matrix3::rotationZ( float radians )
329 {
330 float s, c;
331 s = sinf( radians );
332 c = cosf( radians );
333 return Matrix3(
334 Vector3( c, s, 0.0f ),
335 Vector3( -s, c, 0.0f ),
336 Vector3::zAxis( )
337 );
338 }
339
rotationZYX(const Vector3 & radiansXYZ)340 inline const Matrix3 Matrix3::rotationZYX( const Vector3 & radiansXYZ )
341 {
342 float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
343 sX = sinf( radiansXYZ.getX() );
344 cX = cosf( radiansXYZ.getX() );
345 sY = sinf( radiansXYZ.getY() );
346 cY = cosf( radiansXYZ.getY() );
347 sZ = sinf( radiansXYZ.getZ() );
348 cZ = cosf( radiansXYZ.getZ() );
349 tmp0 = ( cZ * sY );
350 tmp1 = ( sZ * sY );
351 return Matrix3(
352 Vector3( ( cZ * cY ), ( sZ * cY ), -sY ),
353 Vector3( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ) ),
354 Vector3( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ) )
355 );
356 }
357
rotation(float radians,const Vector3 & unitVec)358 inline const Matrix3 Matrix3::rotation( float radians, const Vector3 & unitVec )
359 {
360 float x, y, z, s, c, oneMinusC, xy, yz, zx;
361 s = sinf( radians );
362 c = cosf( radians );
363 x = unitVec.getX();
364 y = unitVec.getY();
365 z = unitVec.getZ();
366 xy = ( x * y );
367 yz = ( y * z );
368 zx = ( z * x );
369 oneMinusC = ( 1.0f - c );
370 return Matrix3(
371 Vector3( ( ( ( x * x ) * oneMinusC ) + c ), ( ( xy * oneMinusC ) + ( z * s ) ), ( ( zx * oneMinusC ) - ( y * s ) ) ),
372 Vector3( ( ( xy * oneMinusC ) - ( z * s ) ), ( ( ( y * y ) * oneMinusC ) + c ), ( ( yz * oneMinusC ) + ( x * s ) ) ),
373 Vector3( ( ( zx * oneMinusC ) + ( y * s ) ), ( ( yz * oneMinusC ) - ( x * s ) ), ( ( ( z * z ) * oneMinusC ) + c ) )
374 );
375 }
376
rotation(const Quat & unitQuat)377 inline const Matrix3 Matrix3::rotation( const Quat & unitQuat )
378 {
379 return Matrix3( unitQuat );
380 }
381
scale(const Vector3 & scaleVec)382 inline const Matrix3 Matrix3::scale( const Vector3 & scaleVec )
383 {
384 return Matrix3(
385 Vector3( scaleVec.getX(), 0.0f, 0.0f ),
386 Vector3( 0.0f, scaleVec.getY(), 0.0f ),
387 Vector3( 0.0f, 0.0f, scaleVec.getZ() )
388 );
389 }
390
appendScale(const Matrix3 & mat,const Vector3 & scaleVec)391 inline const Matrix3 appendScale( const Matrix3 & mat, const Vector3 & scaleVec )
392 {
393 return Matrix3(
394 ( mat.getCol0() * scaleVec.getX( ) ),
395 ( mat.getCol1() * scaleVec.getY( ) ),
396 ( mat.getCol2() * scaleVec.getZ( ) )
397 );
398 }
399
prependScale(const Vector3 & scaleVec,const Matrix3 & mat)400 inline const Matrix3 prependScale( const Vector3 & scaleVec, const Matrix3 & mat )
401 {
402 return Matrix3(
403 mulPerElem( mat.getCol0(), scaleVec ),
404 mulPerElem( mat.getCol1(), scaleVec ),
405 mulPerElem( mat.getCol2(), scaleVec )
406 );
407 }
408
select(const Matrix3 & mat0,const Matrix3 & mat1,bool select1)409 inline const Matrix3 select( const Matrix3 & mat0, const Matrix3 & mat1, bool select1 )
410 {
411 return Matrix3(
412 select( mat0.getCol0(), mat1.getCol0(), select1 ),
413 select( mat0.getCol1(), mat1.getCol1(), select1 ),
414 select( mat0.getCol2(), mat1.getCol2(), select1 )
415 );
416 }
417
418 #ifdef _VECTORMATH_DEBUG
419
print(const Matrix3 & mat)420 inline void print( const Matrix3 & mat )
421 {
422 print( mat.getRow( 0 ) );
423 print( mat.getRow( 1 ) );
424 print( mat.getRow( 2 ) );
425 }
426
print(const Matrix3 & mat,const char * name)427 inline void print( const Matrix3 & mat, const char * name )
428 {
429 printf("%s:\n", name);
430 print( mat );
431 }
432
433 #endif
434
Matrix4(const Matrix4 & mat)435 inline Matrix4::Matrix4( const Matrix4 & mat )
436 {
437 mCol0 = mat.mCol0;
438 mCol1 = mat.mCol1;
439 mCol2 = mat.mCol2;
440 mCol3 = mat.mCol3;
441 }
442
Matrix4(float scalar)443 inline Matrix4::Matrix4( float scalar )
444 {
445 mCol0 = Vector4( scalar );
446 mCol1 = Vector4( scalar );
447 mCol2 = Vector4( scalar );
448 mCol3 = Vector4( scalar );
449 }
450
Matrix4(const Transform3 & mat)451 inline Matrix4::Matrix4( const Transform3 & mat )
452 {
453 mCol0 = Vector4( mat.getCol0(), 0.0f );
454 mCol1 = Vector4( mat.getCol1(), 0.0f );
455 mCol2 = Vector4( mat.getCol2(), 0.0f );
456 mCol3 = Vector4( mat.getCol3(), 1.0f );
457 }
458
Matrix4(const Vector4 & _col0,const Vector4 & _col1,const Vector4 & _col2,const Vector4 & _col3)459 inline Matrix4::Matrix4( const Vector4 & _col0, const Vector4 & _col1, const Vector4 & _col2, const Vector4 & _col3 )
460 {
461 mCol0 = _col0;
462 mCol1 = _col1;
463 mCol2 = _col2;
464 mCol3 = _col3;
465 }
466
Matrix4(const Matrix3 & mat,const Vector3 & translateVec)467 inline Matrix4::Matrix4( const Matrix3 & mat, const Vector3 & translateVec )
468 {
469 mCol0 = Vector4( mat.getCol0(), 0.0f );
470 mCol1 = Vector4( mat.getCol1(), 0.0f );
471 mCol2 = Vector4( mat.getCol2(), 0.0f );
472 mCol3 = Vector4( translateVec, 1.0f );
473 }
474
Matrix4(const Quat & unitQuat,const Vector3 & translateVec)475 inline Matrix4::Matrix4( const Quat & unitQuat, const Vector3 & translateVec )
476 {
477 Matrix3 mat;
478 mat = Matrix3( unitQuat );
479 mCol0 = Vector4( mat.getCol0(), 0.0f );
480 mCol1 = Vector4( mat.getCol1(), 0.0f );
481 mCol2 = Vector4( mat.getCol2(), 0.0f );
482 mCol3 = Vector4( translateVec, 1.0f );
483 }
484
setCol0(const Vector4 & _col0)485 inline Matrix4 & Matrix4::setCol0( const Vector4 & _col0 )
486 {
487 mCol0 = _col0;
488 return *this;
489 }
490
setCol1(const Vector4 & _col1)491 inline Matrix4 & Matrix4::setCol1( const Vector4 & _col1 )
492 {
493 mCol1 = _col1;
494 return *this;
495 }
496
setCol2(const Vector4 & _col2)497 inline Matrix4 & Matrix4::setCol2( const Vector4 & _col2 )
498 {
499 mCol2 = _col2;
500 return *this;
501 }
502
setCol3(const Vector4 & _col3)503 inline Matrix4 & Matrix4::setCol3( const Vector4 & _col3 )
504 {
505 mCol3 = _col3;
506 return *this;
507 }
508
setCol(int col,const Vector4 & vec)509 inline Matrix4 & Matrix4::setCol( int col, const Vector4 & vec )
510 {
511 *(&mCol0 + col) = vec;
512 return *this;
513 }
514
setRow(int row,const Vector4 & vec)515 inline Matrix4 & Matrix4::setRow( int row, const Vector4 & vec )
516 {
517 mCol0.setElem( row, vec.getElem( 0 ) );
518 mCol1.setElem( row, vec.getElem( 1 ) );
519 mCol2.setElem( row, vec.getElem( 2 ) );
520 mCol3.setElem( row, vec.getElem( 3 ) );
521 return *this;
522 }
523
setElem(int col,int row,float val)524 inline Matrix4 & Matrix4::setElem( int col, int row, float val )
525 {
526 Vector4 tmpV3_0;
527 tmpV3_0 = this->getCol( col );
528 tmpV3_0.setElem( row, val );
529 this->setCol( col, tmpV3_0 );
530 return *this;
531 }
532
getElem(int col,int row)533 inline float Matrix4::getElem( int col, int row ) const
534 {
535 return this->getCol( col ).getElem( row );
536 }
537
getCol0()538 inline const Vector4 Matrix4::getCol0( ) const
539 {
540 return mCol0;
541 }
542
getCol1()543 inline const Vector4 Matrix4::getCol1( ) const
544 {
545 return mCol1;
546 }
547
getCol2()548 inline const Vector4 Matrix4::getCol2( ) const
549 {
550 return mCol2;
551 }
552
getCol3()553 inline const Vector4 Matrix4::getCol3( ) const
554 {
555 return mCol3;
556 }
557
getCol(int col)558 inline const Vector4 Matrix4::getCol( int col ) const
559 {
560 return *(&mCol0 + col);
561 }
562
getRow(int row)563 inline const Vector4 Matrix4::getRow( int row ) const
564 {
565 return Vector4( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ), mCol3.getElem( row ) );
566 }
567
568 inline Vector4 & Matrix4::operator []( int col )
569 {
570 return *(&mCol0 + col);
571 }
572
573 inline const Vector4 Matrix4::operator []( int col ) const
574 {
575 return *(&mCol0 + col);
576 }
577
578 inline Matrix4 & Matrix4::operator =( const Matrix4 & mat )
579 {
580 mCol0 = mat.mCol0;
581 mCol1 = mat.mCol1;
582 mCol2 = mat.mCol2;
583 mCol3 = mat.mCol3;
584 return *this;
585 }
586
transpose(const Matrix4 & mat)587 inline const Matrix4 transpose( const Matrix4 & mat )
588 {
589 return Matrix4(
590 Vector4( mat.getCol0().getX(), mat.getCol1().getX(), mat.getCol2().getX(), mat.getCol3().getX() ),
591 Vector4( mat.getCol0().getY(), mat.getCol1().getY(), mat.getCol2().getY(), mat.getCol3().getY() ),
592 Vector4( mat.getCol0().getZ(), mat.getCol1().getZ(), mat.getCol2().getZ(), mat.getCol3().getZ() ),
593 Vector4( mat.getCol0().getW(), mat.getCol1().getW(), mat.getCol2().getW(), mat.getCol3().getW() )
594 );
595 }
596
inverse(const Matrix4 & mat)597 inline const Matrix4 inverse( const Matrix4 & mat )
598 {
599 Vector4 res0, res1, res2, res3;
600 float mA, mB, mC, mD, mE, mF, mG, mH, mI, mJ, mK, mL, mM, mN, mO, mP, tmp0, tmp1, tmp2, tmp3, tmp4, tmp5, detInv;
601 mA = mat.getCol0().getX();
602 mB = mat.getCol0().getY();
603 mC = mat.getCol0().getZ();
604 mD = mat.getCol0().getW();
605 mE = mat.getCol1().getX();
606 mF = mat.getCol1().getY();
607 mG = mat.getCol1().getZ();
608 mH = mat.getCol1().getW();
609 mI = mat.getCol2().getX();
610 mJ = mat.getCol2().getY();
611 mK = mat.getCol2().getZ();
612 mL = mat.getCol2().getW();
613 mM = mat.getCol3().getX();
614 mN = mat.getCol3().getY();
615 mO = mat.getCol3().getZ();
616 mP = mat.getCol3().getW();
617 tmp0 = ( ( mK * mD ) - ( mC * mL ) );
618 tmp1 = ( ( mO * mH ) - ( mG * mP ) );
619 tmp2 = ( ( mB * mK ) - ( mJ * mC ) );
620 tmp3 = ( ( mF * mO ) - ( mN * mG ) );
621 tmp4 = ( ( mJ * mD ) - ( mB * mL ) );
622 tmp5 = ( ( mN * mH ) - ( mF * mP ) );
623 res0.setX( ( ( ( mJ * tmp1 ) - ( mL * tmp3 ) ) - ( mK * tmp5 ) ) );
624 res0.setY( ( ( ( mN * tmp0 ) - ( mP * tmp2 ) ) - ( mO * tmp4 ) ) );
625 res0.setZ( ( ( ( mD * tmp3 ) + ( mC * tmp5 ) ) - ( mB * tmp1 ) ) );
626 res0.setW( ( ( ( mH * tmp2 ) + ( mG * tmp4 ) ) - ( mF * tmp0 ) ) );
627 detInv = ( 1.0f / ( ( ( ( mA * res0.getX() ) + ( mE * res0.getY() ) ) + ( mI * res0.getZ() ) ) + ( mM * res0.getW() ) ) );
628 res1.setX( ( mI * tmp1 ) );
629 res1.setY( ( mM * tmp0 ) );
630 res1.setZ( ( mA * tmp1 ) );
631 res1.setW( ( mE * tmp0 ) );
632 res3.setX( ( mI * tmp3 ) );
633 res3.setY( ( mM * tmp2 ) );
634 res3.setZ( ( mA * tmp3 ) );
635 res3.setW( ( mE * tmp2 ) );
636 res2.setX( ( mI * tmp5 ) );
637 res2.setY( ( mM * tmp4 ) );
638 res2.setZ( ( mA * tmp5 ) );
639 res2.setW( ( mE * tmp4 ) );
640 tmp0 = ( ( mI * mB ) - ( mA * mJ ) );
641 tmp1 = ( ( mM * mF ) - ( mE * mN ) );
642 tmp2 = ( ( mI * mD ) - ( mA * mL ) );
643 tmp3 = ( ( mM * mH ) - ( mE * mP ) );
644 tmp4 = ( ( mI * mC ) - ( mA * mK ) );
645 tmp5 = ( ( mM * mG ) - ( mE * mO ) );
646 res2.setX( ( ( ( mL * tmp1 ) - ( mJ * tmp3 ) ) + res2.getX() ) );
647 res2.setY( ( ( ( mP * tmp0 ) - ( mN * tmp2 ) ) + res2.getY() ) );
648 res2.setZ( ( ( ( mB * tmp3 ) - ( mD * tmp1 ) ) - res2.getZ() ) );
649 res2.setW( ( ( ( mF * tmp2 ) - ( mH * tmp0 ) ) - res2.getW() ) );
650 res3.setX( ( ( ( mJ * tmp5 ) - ( mK * tmp1 ) ) + res3.getX() ) );
651 res3.setY( ( ( ( mN * tmp4 ) - ( mO * tmp0 ) ) + res3.getY() ) );
652 res3.setZ( ( ( ( mC * tmp1 ) - ( mB * tmp5 ) ) - res3.getZ() ) );
653 res3.setW( ( ( ( mG * tmp0 ) - ( mF * tmp4 ) ) - res3.getW() ) );
654 res1.setX( ( ( ( mK * tmp3 ) - ( mL * tmp5 ) ) - res1.getX() ) );
655 res1.setY( ( ( ( mO * tmp2 ) - ( mP * tmp4 ) ) - res1.getY() ) );
656 res1.setZ( ( ( ( mD * tmp5 ) - ( mC * tmp3 ) ) + res1.getZ() ) );
657 res1.setW( ( ( ( mH * tmp4 ) - ( mG * tmp2 ) ) + res1.getW() ) );
658 return Matrix4(
659 ( res0 * detInv ),
660 ( res1 * detInv ),
661 ( res2 * detInv ),
662 ( res3 * detInv )
663 );
664 }
665
affineInverse(const Matrix4 & mat)666 inline const Matrix4 affineInverse( const Matrix4 & mat )
667 {
668 Transform3 affineMat;
669 affineMat.setCol0( mat.getCol0().getXYZ( ) );
670 affineMat.setCol1( mat.getCol1().getXYZ( ) );
671 affineMat.setCol2( mat.getCol2().getXYZ( ) );
672 affineMat.setCol3( mat.getCol3().getXYZ( ) );
673 return Matrix4( inverse( affineMat ) );
674 }
675
orthoInverse(const Matrix4 & mat)676 inline const Matrix4 orthoInverse( const Matrix4 & mat )
677 {
678 Transform3 affineMat;
679 affineMat.setCol0( mat.getCol0().getXYZ( ) );
680 affineMat.setCol1( mat.getCol1().getXYZ( ) );
681 affineMat.setCol2( mat.getCol2().getXYZ( ) );
682 affineMat.setCol3( mat.getCol3().getXYZ( ) );
683 return Matrix4( orthoInverse( affineMat ) );
684 }
685
determinant(const Matrix4 & mat)686 inline float determinant( const Matrix4 & mat )
687 {
688 float dx, dy, dz, dw, mA, mB, mC, mD, mE, mF, mG, mH, mI, mJ, mK, mL, mM, mN, mO, mP, tmp0, tmp1, tmp2, tmp3, tmp4, tmp5;
689 mA = mat.getCol0().getX();
690 mB = mat.getCol0().getY();
691 mC = mat.getCol0().getZ();
692 mD = mat.getCol0().getW();
693 mE = mat.getCol1().getX();
694 mF = mat.getCol1().getY();
695 mG = mat.getCol1().getZ();
696 mH = mat.getCol1().getW();
697 mI = mat.getCol2().getX();
698 mJ = mat.getCol2().getY();
699 mK = mat.getCol2().getZ();
700 mL = mat.getCol2().getW();
701 mM = mat.getCol3().getX();
702 mN = mat.getCol3().getY();
703 mO = mat.getCol3().getZ();
704 mP = mat.getCol3().getW();
705 tmp0 = ( ( mK * mD ) - ( mC * mL ) );
706 tmp1 = ( ( mO * mH ) - ( mG * mP ) );
707 tmp2 = ( ( mB * mK ) - ( mJ * mC ) );
708 tmp3 = ( ( mF * mO ) - ( mN * mG ) );
709 tmp4 = ( ( mJ * mD ) - ( mB * mL ) );
710 tmp5 = ( ( mN * mH ) - ( mF * mP ) );
711 dx = ( ( ( mJ * tmp1 ) - ( mL * tmp3 ) ) - ( mK * tmp5 ) );
712 dy = ( ( ( mN * tmp0 ) - ( mP * tmp2 ) ) - ( mO * tmp4 ) );
713 dz = ( ( ( mD * tmp3 ) + ( mC * tmp5 ) ) - ( mB * tmp1 ) );
714 dw = ( ( ( mH * tmp2 ) + ( mG * tmp4 ) ) - ( mF * tmp0 ) );
715 return ( ( ( ( mA * dx ) + ( mE * dy ) ) + ( mI * dz ) ) + ( mM * dw ) );
716 }
717
718 inline const Matrix4 Matrix4::operator +( const Matrix4 & mat ) const
719 {
720 return Matrix4(
721 ( mCol0 + mat.mCol0 ),
722 ( mCol1 + mat.mCol1 ),
723 ( mCol2 + mat.mCol2 ),
724 ( mCol3 + mat.mCol3 )
725 );
726 }
727
728 inline const Matrix4 Matrix4::operator -( const Matrix4 & mat ) const
729 {
730 return Matrix4(
731 ( mCol0 - mat.mCol0 ),
732 ( mCol1 - mat.mCol1 ),
733 ( mCol2 - mat.mCol2 ),
734 ( mCol3 - mat.mCol3 )
735 );
736 }
737
738 inline Matrix4 & Matrix4::operator +=( const Matrix4 & mat )
739 {
740 *this = *this + mat;
741 return *this;
742 }
743
744 inline Matrix4 & Matrix4::operator -=( const Matrix4 & mat )
745 {
746 *this = *this - mat;
747 return *this;
748 }
749
750 inline const Matrix4 Matrix4::operator -( ) const
751 {
752 return Matrix4(
753 ( -mCol0 ),
754 ( -mCol1 ),
755 ( -mCol2 ),
756 ( -mCol3 )
757 );
758 }
759
absPerElem(const Matrix4 & mat)760 inline const Matrix4 absPerElem( const Matrix4 & mat )
761 {
762 return Matrix4(
763 absPerElem( mat.getCol0() ),
764 absPerElem( mat.getCol1() ),
765 absPerElem( mat.getCol2() ),
766 absPerElem( mat.getCol3() )
767 );
768 }
769
770 inline const Matrix4 Matrix4::operator *( float scalar ) const
771 {
772 return Matrix4(
773 ( mCol0 * scalar ),
774 ( mCol1 * scalar ),
775 ( mCol2 * scalar ),
776 ( mCol3 * scalar )
777 );
778 }
779
780 inline Matrix4 & Matrix4::operator *=( float scalar )
781 {
782 *this = *this * scalar;
783 return *this;
784 }
785
786 inline const Matrix4 operator *( float scalar, const Matrix4 & mat )
787 {
788 return mat * scalar;
789 }
790
791 inline const Vector4 Matrix4::operator *( const Vector4 & vec ) const
792 {
793 return Vector4(
794 ( ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ) + ( mCol3.getX() * vec.getW() ) ),
795 ( ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ) + ( mCol3.getY() * vec.getW() ) ),
796 ( ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) ) + ( mCol3.getZ() * vec.getW() ) ),
797 ( ( ( ( mCol0.getW() * vec.getX() ) + ( mCol1.getW() * vec.getY() ) ) + ( mCol2.getW() * vec.getZ() ) ) + ( mCol3.getW() * vec.getW() ) )
798 );
799 }
800
801 inline const Vector4 Matrix4::operator *( const Vector3 & vec ) const
802 {
803 return Vector4(
804 ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
805 ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
806 ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) ),
807 ( ( ( mCol0.getW() * vec.getX() ) + ( mCol1.getW() * vec.getY() ) ) + ( mCol2.getW() * vec.getZ() ) )
808 );
809 }
810
811 inline const Vector4 Matrix4::operator *( const Point3 & pnt ) const
812 {
813 return Vector4(
814 ( ( ( ( mCol0.getX() * pnt.getX() ) + ( mCol1.getX() * pnt.getY() ) ) + ( mCol2.getX() * pnt.getZ() ) ) + mCol3.getX() ),
815 ( ( ( ( mCol0.getY() * pnt.getX() ) + ( mCol1.getY() * pnt.getY() ) ) + ( mCol2.getY() * pnt.getZ() ) ) + mCol3.getY() ),
816 ( ( ( ( mCol0.getZ() * pnt.getX() ) + ( mCol1.getZ() * pnt.getY() ) ) + ( mCol2.getZ() * pnt.getZ() ) ) + mCol3.getZ() ),
817 ( ( ( ( mCol0.getW() * pnt.getX() ) + ( mCol1.getW() * pnt.getY() ) ) + ( mCol2.getW() * pnt.getZ() ) ) + mCol3.getW() )
818 );
819 }
820
821 inline const Matrix4 Matrix4::operator *( const Matrix4 & mat ) const
822 {
823 return Matrix4(
824 ( *this * mat.mCol0 ),
825 ( *this * mat.mCol1 ),
826 ( *this * mat.mCol2 ),
827 ( *this * mat.mCol3 )
828 );
829 }
830
831 inline Matrix4 & Matrix4::operator *=( const Matrix4 & mat )
832 {
833 *this = *this * mat;
834 return *this;
835 }
836
837 inline const Matrix4 Matrix4::operator *( const Transform3 & tfrm ) const
838 {
839 return Matrix4(
840 ( *this * tfrm.getCol0() ),
841 ( *this * tfrm.getCol1() ),
842 ( *this * tfrm.getCol2() ),
843 ( *this * Point3( tfrm.getCol3() ) )
844 );
845 }
846
847 inline Matrix4 & Matrix4::operator *=( const Transform3 & tfrm )
848 {
849 *this = *this * tfrm;
850 return *this;
851 }
852
mulPerElem(const Matrix4 & mat0,const Matrix4 & mat1)853 inline const Matrix4 mulPerElem( const Matrix4 & mat0, const Matrix4 & mat1 )
854 {
855 return Matrix4(
856 mulPerElem( mat0.getCol0(), mat1.getCol0() ),
857 mulPerElem( mat0.getCol1(), mat1.getCol1() ),
858 mulPerElem( mat0.getCol2(), mat1.getCol2() ),
859 mulPerElem( mat0.getCol3(), mat1.getCol3() )
860 );
861 }
862
identity()863 inline const Matrix4 Matrix4::identity( )
864 {
865 return Matrix4(
866 Vector4::xAxis( ),
867 Vector4::yAxis( ),
868 Vector4::zAxis( ),
869 Vector4::wAxis( )
870 );
871 }
872
setUpper3x3(const Matrix3 & mat3)873 inline Matrix4 & Matrix4::setUpper3x3( const Matrix3 & mat3 )
874 {
875 mCol0.setXYZ( mat3.getCol0() );
876 mCol1.setXYZ( mat3.getCol1() );
877 mCol2.setXYZ( mat3.getCol2() );
878 return *this;
879 }
880
getUpper3x3()881 inline const Matrix3 Matrix4::getUpper3x3( ) const
882 {
883 return Matrix3(
884 mCol0.getXYZ( ),
885 mCol1.getXYZ( ),
886 mCol2.getXYZ( )
887 );
888 }
889
setTranslation(const Vector3 & translateVec)890 inline Matrix4 & Matrix4::setTranslation( const Vector3 & translateVec )
891 {
892 mCol3.setXYZ( translateVec );
893 return *this;
894 }
895
getTranslation()896 inline const Vector3 Matrix4::getTranslation( ) const
897 {
898 return mCol3.getXYZ( );
899 }
900
rotationX(float radians)901 inline const Matrix4 Matrix4::rotationX( float radians )
902 {
903 float s, c;
904 s = sinf( radians );
905 c = cosf( radians );
906 return Matrix4(
907 Vector4::xAxis( ),
908 Vector4( 0.0f, c, s, 0.0f ),
909 Vector4( 0.0f, -s, c, 0.0f ),
910 Vector4::wAxis( )
911 );
912 }
913
rotationY(float radians)914 inline const Matrix4 Matrix4::rotationY( float radians )
915 {
916 float s, c;
917 s = sinf( radians );
918 c = cosf( radians );
919 return Matrix4(
920 Vector4( c, 0.0f, -s, 0.0f ),
921 Vector4::yAxis( ),
922 Vector4( s, 0.0f, c, 0.0f ),
923 Vector4::wAxis( )
924 );
925 }
926
rotationZ(float radians)927 inline const Matrix4 Matrix4::rotationZ( float radians )
928 {
929 float s, c;
930 s = sinf( radians );
931 c = cosf( radians );
932 return Matrix4(
933 Vector4( c, s, 0.0f, 0.0f ),
934 Vector4( -s, c, 0.0f, 0.0f ),
935 Vector4::zAxis( ),
936 Vector4::wAxis( )
937 );
938 }
939
rotationZYX(const Vector3 & radiansXYZ)940 inline const Matrix4 Matrix4::rotationZYX( const Vector3 & radiansXYZ )
941 {
942 float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
943 sX = sinf( radiansXYZ.getX() );
944 cX = cosf( radiansXYZ.getX() );
945 sY = sinf( radiansXYZ.getY() );
946 cY = cosf( radiansXYZ.getY() );
947 sZ = sinf( radiansXYZ.getZ() );
948 cZ = cosf( radiansXYZ.getZ() );
949 tmp0 = ( cZ * sY );
950 tmp1 = ( sZ * sY );
951 return Matrix4(
952 Vector4( ( cZ * cY ), ( sZ * cY ), -sY, 0.0f ),
953 Vector4( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ), 0.0f ),
954 Vector4( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ), 0.0f ),
955 Vector4::wAxis( )
956 );
957 }
958
rotation(float radians,const Vector3 & unitVec)959 inline const Matrix4 Matrix4::rotation( float radians, const Vector3 & unitVec )
960 {
961 float x, y, z, s, c, oneMinusC, xy, yz, zx;
962 s = sinf( radians );
963 c = cosf( radians );
964 x = unitVec.getX();
965 y = unitVec.getY();
966 z = unitVec.getZ();
967 xy = ( x * y );
968 yz = ( y * z );
969 zx = ( z * x );
970 oneMinusC = ( 1.0f - c );
971 return Matrix4(
972 Vector4( ( ( ( x * x ) * oneMinusC ) + c ), ( ( xy * oneMinusC ) + ( z * s ) ), ( ( zx * oneMinusC ) - ( y * s ) ), 0.0f ),
973 Vector4( ( ( xy * oneMinusC ) - ( z * s ) ), ( ( ( y * y ) * oneMinusC ) + c ), ( ( yz * oneMinusC ) + ( x * s ) ), 0.0f ),
974 Vector4( ( ( zx * oneMinusC ) + ( y * s ) ), ( ( yz * oneMinusC ) - ( x * s ) ), ( ( ( z * z ) * oneMinusC ) + c ), 0.0f ),
975 Vector4::wAxis( )
976 );
977 }
978
rotation(const Quat & unitQuat)979 inline const Matrix4 Matrix4::rotation( const Quat & unitQuat )
980 {
981 return Matrix4( Transform3::rotation( unitQuat ) );
982 }
983
scale(const Vector3 & scaleVec)984 inline const Matrix4 Matrix4::scale( const Vector3 & scaleVec )
985 {
986 return Matrix4(
987 Vector4( scaleVec.getX(), 0.0f, 0.0f, 0.0f ),
988 Vector4( 0.0f, scaleVec.getY(), 0.0f, 0.0f ),
989 Vector4( 0.0f, 0.0f, scaleVec.getZ(), 0.0f ),
990 Vector4::wAxis( )
991 );
992 }
993
appendScale(const Matrix4 & mat,const Vector3 & scaleVec)994 inline const Matrix4 appendScale( const Matrix4 & mat, const Vector3 & scaleVec )
995 {
996 return Matrix4(
997 ( mat.getCol0() * scaleVec.getX( ) ),
998 ( mat.getCol1() * scaleVec.getY( ) ),
999 ( mat.getCol2() * scaleVec.getZ( ) ),
1000 mat.getCol3()
1001 );
1002 }
1003
prependScale(const Vector3 & scaleVec,const Matrix4 & mat)1004 inline const Matrix4 prependScale( const Vector3 & scaleVec, const Matrix4 & mat )
1005 {
1006 Vector4 scale4;
1007 scale4 = Vector4( scaleVec, 1.0f );
1008 return Matrix4(
1009 mulPerElem( mat.getCol0(), scale4 ),
1010 mulPerElem( mat.getCol1(), scale4 ),
1011 mulPerElem( mat.getCol2(), scale4 ),
1012 mulPerElem( mat.getCol3(), scale4 )
1013 );
1014 }
1015
translation(const Vector3 & translateVec)1016 inline const Matrix4 Matrix4::translation( const Vector3 & translateVec )
1017 {
1018 return Matrix4(
1019 Vector4::xAxis( ),
1020 Vector4::yAxis( ),
1021 Vector4::zAxis( ),
1022 Vector4( translateVec, 1.0f )
1023 );
1024 }
1025
lookAt(const Point3 & eyePos,const Point3 & lookAtPos,const Vector3 & upVec)1026 inline const Matrix4 Matrix4::lookAt( const Point3 & eyePos, const Point3 & lookAtPos, const Vector3 & upVec )
1027 {
1028 Matrix4 m4EyeFrame;
1029 Vector3 v3X, v3Y, v3Z;
1030 v3Y = normalize( upVec );
1031 v3Z = normalize( ( eyePos - lookAtPos ) );
1032 v3X = normalize( cross( v3Y, v3Z ) );
1033 v3Y = cross( v3Z, v3X );
1034 m4EyeFrame = Matrix4( Vector4( v3X ), Vector4( v3Y ), Vector4( v3Z ), Vector4( eyePos ) );
1035 return orthoInverse( m4EyeFrame );
1036 }
1037
perspective(float fovyRadians,float aspect,float zNear,float zFar)1038 inline const Matrix4 Matrix4::perspective( float fovyRadians, float aspect, float zNear, float zFar )
1039 {
1040 float f, rangeInv;
1041 f = tanf( ( (float)( _VECTORMATH_PI_OVER_2 ) - ( 0.5f * fovyRadians ) ) );
1042 rangeInv = ( 1.0f / ( zNear - zFar ) );
1043 return Matrix4(
1044 Vector4( ( f / aspect ), 0.0f, 0.0f, 0.0f ),
1045 Vector4( 0.0f, f, 0.0f, 0.0f ),
1046 Vector4( 0.0f, 0.0f, ( ( zNear + zFar ) * rangeInv ), -1.0f ),
1047 Vector4( 0.0f, 0.0f, ( ( ( zNear * zFar ) * rangeInv ) * 2.0f ), 0.0f )
1048 );
1049 }
1050
frustum(float left,float right,float bottom,float top,float zNear,float zFar)1051 inline const Matrix4 Matrix4::frustum( float left, float right, float bottom, float top, float zNear, float zFar )
1052 {
1053 float sum_rl, sum_tb, sum_nf, inv_rl, inv_tb, inv_nf, n2;
1054 sum_rl = ( right + left );
1055 sum_tb = ( top + bottom );
1056 sum_nf = ( zNear + zFar );
1057 inv_rl = ( 1.0f / ( right - left ) );
1058 inv_tb = ( 1.0f / ( top - bottom ) );
1059 inv_nf = ( 1.0f / ( zNear - zFar ) );
1060 n2 = ( zNear + zNear );
1061 return Matrix4(
1062 Vector4( ( n2 * inv_rl ), 0.0f, 0.0f, 0.0f ),
1063 Vector4( 0.0f, ( n2 * inv_tb ), 0.0f, 0.0f ),
1064 Vector4( ( sum_rl * inv_rl ), ( sum_tb * inv_tb ), ( sum_nf * inv_nf ), -1.0f ),
1065 Vector4( 0.0f, 0.0f, ( ( n2 * inv_nf ) * zFar ), 0.0f )
1066 );
1067 }
1068
orthographic(float left,float right,float bottom,float top,float zNear,float zFar)1069 inline const Matrix4 Matrix4::orthographic( float left, float right, float bottom, float top, float zNear, float zFar )
1070 {
1071 float sum_rl, sum_tb, sum_nf, inv_rl, inv_tb, inv_nf;
1072 sum_rl = ( right + left );
1073 sum_tb = ( top + bottom );
1074 sum_nf = ( zNear + zFar );
1075 inv_rl = ( 1.0f / ( right - left ) );
1076 inv_tb = ( 1.0f / ( top - bottom ) );
1077 inv_nf = ( 1.0f / ( zNear - zFar ) );
1078 return Matrix4(
1079 Vector4( ( inv_rl + inv_rl ), 0.0f, 0.0f, 0.0f ),
1080 Vector4( 0.0f, ( inv_tb + inv_tb ), 0.0f, 0.0f ),
1081 Vector4( 0.0f, 0.0f, ( inv_nf + inv_nf ), 0.0f ),
1082 Vector4( ( -sum_rl * inv_rl ), ( -sum_tb * inv_tb ), ( sum_nf * inv_nf ), 1.0f )
1083 );
1084 }
1085
select(const Matrix4 & mat0,const Matrix4 & mat1,bool select1)1086 inline const Matrix4 select( const Matrix4 & mat0, const Matrix4 & mat1, bool select1 )
1087 {
1088 return Matrix4(
1089 select( mat0.getCol0(), mat1.getCol0(), select1 ),
1090 select( mat0.getCol1(), mat1.getCol1(), select1 ),
1091 select( mat0.getCol2(), mat1.getCol2(), select1 ),
1092 select( mat0.getCol3(), mat1.getCol3(), select1 )
1093 );
1094 }
1095
1096 #ifdef _VECTORMATH_DEBUG
1097
print(const Matrix4 & mat)1098 inline void print( const Matrix4 & mat )
1099 {
1100 print( mat.getRow( 0 ) );
1101 print( mat.getRow( 1 ) );
1102 print( mat.getRow( 2 ) );
1103 print( mat.getRow( 3 ) );
1104 }
1105
print(const Matrix4 & mat,const char * name)1106 inline void print( const Matrix4 & mat, const char * name )
1107 {
1108 printf("%s:\n", name);
1109 print( mat );
1110 }
1111
1112 #endif
1113
Transform3(const Transform3 & tfrm)1114 inline Transform3::Transform3( const Transform3 & tfrm )
1115 {
1116 mCol0 = tfrm.mCol0;
1117 mCol1 = tfrm.mCol1;
1118 mCol2 = tfrm.mCol2;
1119 mCol3 = tfrm.mCol3;
1120 }
1121
Transform3(float scalar)1122 inline Transform3::Transform3( float scalar )
1123 {
1124 mCol0 = Vector3( scalar );
1125 mCol1 = Vector3( scalar );
1126 mCol2 = Vector3( scalar );
1127 mCol3 = Vector3( scalar );
1128 }
1129
Transform3(const Vector3 & _col0,const Vector3 & _col1,const Vector3 & _col2,const Vector3 & _col3)1130 inline Transform3::Transform3( const Vector3 & _col0, const Vector3 & _col1, const Vector3 & _col2, const Vector3 & _col3 )
1131 {
1132 mCol0 = _col0;
1133 mCol1 = _col1;
1134 mCol2 = _col2;
1135 mCol3 = _col3;
1136 }
1137
Transform3(const Matrix3 & tfrm,const Vector3 & translateVec)1138 inline Transform3::Transform3( const Matrix3 & tfrm, const Vector3 & translateVec )
1139 {
1140 this->setUpper3x3( tfrm );
1141 this->setTranslation( translateVec );
1142 }
1143
Transform3(const Quat & unitQuat,const Vector3 & translateVec)1144 inline Transform3::Transform3( const Quat & unitQuat, const Vector3 & translateVec )
1145 {
1146 this->setUpper3x3( Matrix3( unitQuat ) );
1147 this->setTranslation( translateVec );
1148 }
1149
setCol0(const Vector3 & _col0)1150 inline Transform3 & Transform3::setCol0( const Vector3 & _col0 )
1151 {
1152 mCol0 = _col0;
1153 return *this;
1154 }
1155
setCol1(const Vector3 & _col1)1156 inline Transform3 & Transform3::setCol1( const Vector3 & _col1 )
1157 {
1158 mCol1 = _col1;
1159 return *this;
1160 }
1161
setCol2(const Vector3 & _col2)1162 inline Transform3 & Transform3::setCol2( const Vector3 & _col2 )
1163 {
1164 mCol2 = _col2;
1165 return *this;
1166 }
1167
setCol3(const Vector3 & _col3)1168 inline Transform3 & Transform3::setCol3( const Vector3 & _col3 )
1169 {
1170 mCol3 = _col3;
1171 return *this;
1172 }
1173
setCol(int col,const Vector3 & vec)1174 inline Transform3 & Transform3::setCol( int col, const Vector3 & vec )
1175 {
1176 *(&mCol0 + col) = vec;
1177 return *this;
1178 }
1179
setRow(int row,const Vector4 & vec)1180 inline Transform3 & Transform3::setRow( int row, const Vector4 & vec )
1181 {
1182 mCol0.setElem( row, vec.getElem( 0 ) );
1183 mCol1.setElem( row, vec.getElem( 1 ) );
1184 mCol2.setElem( row, vec.getElem( 2 ) );
1185 mCol3.setElem( row, vec.getElem( 3 ) );
1186 return *this;
1187 }
1188
setElem(int col,int row,float val)1189 inline Transform3 & Transform3::setElem( int col, int row, float val )
1190 {
1191 Vector3 tmpV3_0;
1192 tmpV3_0 = this->getCol( col );
1193 tmpV3_0.setElem( row, val );
1194 this->setCol( col, tmpV3_0 );
1195 return *this;
1196 }
1197
getElem(int col,int row)1198 inline float Transform3::getElem( int col, int row ) const
1199 {
1200 return this->getCol( col ).getElem( row );
1201 }
1202
getCol0()1203 inline const Vector3 Transform3::getCol0( ) const
1204 {
1205 return mCol0;
1206 }
1207
getCol1()1208 inline const Vector3 Transform3::getCol1( ) const
1209 {
1210 return mCol1;
1211 }
1212
getCol2()1213 inline const Vector3 Transform3::getCol2( ) const
1214 {
1215 return mCol2;
1216 }
1217
getCol3()1218 inline const Vector3 Transform3::getCol3( ) const
1219 {
1220 return mCol3;
1221 }
1222
getCol(int col)1223 inline const Vector3 Transform3::getCol( int col ) const
1224 {
1225 return *(&mCol0 + col);
1226 }
1227
getRow(int row)1228 inline const Vector4 Transform3::getRow( int row ) const
1229 {
1230 return Vector4( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ), mCol3.getElem( row ) );
1231 }
1232
1233 inline Vector3 & Transform3::operator []( int col )
1234 {
1235 return *(&mCol0 + col);
1236 }
1237
1238 inline const Vector3 Transform3::operator []( int col ) const
1239 {
1240 return *(&mCol0 + col);
1241 }
1242
1243 inline Transform3 & Transform3::operator =( const Transform3 & tfrm )
1244 {
1245 mCol0 = tfrm.mCol0;
1246 mCol1 = tfrm.mCol1;
1247 mCol2 = tfrm.mCol2;
1248 mCol3 = tfrm.mCol3;
1249 return *this;
1250 }
1251
inverse(const Transform3 & tfrm)1252 inline const Transform3 inverse( const Transform3 & tfrm )
1253 {
1254 Vector3 tmp0, tmp1, tmp2, inv0, inv1, inv2;
1255 float detinv;
1256 tmp0 = cross( tfrm.getCol1(), tfrm.getCol2() );
1257 tmp1 = cross( tfrm.getCol2(), tfrm.getCol0() );
1258 tmp2 = cross( tfrm.getCol0(), tfrm.getCol1() );
1259 detinv = ( 1.0f / dot( tfrm.getCol2(), tmp2 ) );
1260 inv0 = Vector3( ( tmp0.getX() * detinv ), ( tmp1.getX() * detinv ), ( tmp2.getX() * detinv ) );
1261 inv1 = Vector3( ( tmp0.getY() * detinv ), ( tmp1.getY() * detinv ), ( tmp2.getY() * detinv ) );
1262 inv2 = Vector3( ( tmp0.getZ() * detinv ), ( tmp1.getZ() * detinv ), ( tmp2.getZ() * detinv ) );
1263 return Transform3(
1264 inv0,
1265 inv1,
1266 inv2,
1267 Vector3( ( -( ( inv0 * tfrm.getCol3().getX() ) + ( ( inv1 * tfrm.getCol3().getY() ) + ( inv2 * tfrm.getCol3().getZ() ) ) ) ) )
1268 );
1269 }
1270
orthoInverse(const Transform3 & tfrm)1271 inline const Transform3 orthoInverse( const Transform3 & tfrm )
1272 {
1273 Vector3 inv0, inv1, inv2;
1274 inv0 = Vector3( tfrm.getCol0().getX(), tfrm.getCol1().getX(), tfrm.getCol2().getX() );
1275 inv1 = Vector3( tfrm.getCol0().getY(), tfrm.getCol1().getY(), tfrm.getCol2().getY() );
1276 inv2 = Vector3( tfrm.getCol0().getZ(), tfrm.getCol1().getZ(), tfrm.getCol2().getZ() );
1277 return Transform3(
1278 inv0,
1279 inv1,
1280 inv2,
1281 Vector3( ( -( ( inv0 * tfrm.getCol3().getX() ) + ( ( inv1 * tfrm.getCol3().getY() ) + ( inv2 * tfrm.getCol3().getZ() ) ) ) ) )
1282 );
1283 }
1284
absPerElem(const Transform3 & tfrm)1285 inline const Transform3 absPerElem( const Transform3 & tfrm )
1286 {
1287 return Transform3(
1288 absPerElem( tfrm.getCol0() ),
1289 absPerElem( tfrm.getCol1() ),
1290 absPerElem( tfrm.getCol2() ),
1291 absPerElem( tfrm.getCol3() )
1292 );
1293 }
1294
1295 inline const Vector3 Transform3::operator *( const Vector3 & vec ) const
1296 {
1297 return Vector3(
1298 ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
1299 ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
1300 ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) )
1301 );
1302 }
1303
1304 inline const Point3 Transform3::operator *( const Point3 & pnt ) const
1305 {
1306 return Point3(
1307 ( ( ( ( mCol0.getX() * pnt.getX() ) + ( mCol1.getX() * pnt.getY() ) ) + ( mCol2.getX() * pnt.getZ() ) ) + mCol3.getX() ),
1308 ( ( ( ( mCol0.getY() * pnt.getX() ) + ( mCol1.getY() * pnt.getY() ) ) + ( mCol2.getY() * pnt.getZ() ) ) + mCol3.getY() ),
1309 ( ( ( ( mCol0.getZ() * pnt.getX() ) + ( mCol1.getZ() * pnt.getY() ) ) + ( mCol2.getZ() * pnt.getZ() ) ) + mCol3.getZ() )
1310 );
1311 }
1312
1313 inline const Transform3 Transform3::operator *( const Transform3 & tfrm ) const
1314 {
1315 return Transform3(
1316 ( *this * tfrm.mCol0 ),
1317 ( *this * tfrm.mCol1 ),
1318 ( *this * tfrm.mCol2 ),
1319 Vector3( ( *this * Point3( tfrm.mCol3 ) ) )
1320 );
1321 }
1322
1323 inline Transform3 & Transform3::operator *=( const Transform3 & tfrm )
1324 {
1325 *this = *this * tfrm;
1326 return *this;
1327 }
1328
mulPerElem(const Transform3 & tfrm0,const Transform3 & tfrm1)1329 inline const Transform3 mulPerElem( const Transform3 & tfrm0, const Transform3 & tfrm1 )
1330 {
1331 return Transform3(
1332 mulPerElem( tfrm0.getCol0(), tfrm1.getCol0() ),
1333 mulPerElem( tfrm0.getCol1(), tfrm1.getCol1() ),
1334 mulPerElem( tfrm0.getCol2(), tfrm1.getCol2() ),
1335 mulPerElem( tfrm0.getCol3(), tfrm1.getCol3() )
1336 );
1337 }
1338
identity()1339 inline const Transform3 Transform3::identity( )
1340 {
1341 return Transform3(
1342 Vector3::xAxis( ),
1343 Vector3::yAxis( ),
1344 Vector3::zAxis( ),
1345 Vector3( 0.0f )
1346 );
1347 }
1348
setUpper3x3(const Matrix3 & tfrm)1349 inline Transform3 & Transform3::setUpper3x3( const Matrix3 & tfrm )
1350 {
1351 mCol0 = tfrm.getCol0();
1352 mCol1 = tfrm.getCol1();
1353 mCol2 = tfrm.getCol2();
1354 return *this;
1355 }
1356
getUpper3x3()1357 inline const Matrix3 Transform3::getUpper3x3( ) const
1358 {
1359 return Matrix3( mCol0, mCol1, mCol2 );
1360 }
1361
setTranslation(const Vector3 & translateVec)1362 inline Transform3 & Transform3::setTranslation( const Vector3 & translateVec )
1363 {
1364 mCol3 = translateVec;
1365 return *this;
1366 }
1367
getTranslation()1368 inline const Vector3 Transform3::getTranslation( ) const
1369 {
1370 return mCol3;
1371 }
1372
rotationX(float radians)1373 inline const Transform3 Transform3::rotationX( float radians )
1374 {
1375 float s, c;
1376 s = sinf( radians );
1377 c = cosf( radians );
1378 return Transform3(
1379 Vector3::xAxis( ),
1380 Vector3( 0.0f, c, s ),
1381 Vector3( 0.0f, -s, c ),
1382 Vector3( 0.0f )
1383 );
1384 }
1385
rotationY(float radians)1386 inline const Transform3 Transform3::rotationY( float radians )
1387 {
1388 float s, c;
1389 s = sinf( radians );
1390 c = cosf( radians );
1391 return Transform3(
1392 Vector3( c, 0.0f, -s ),
1393 Vector3::yAxis( ),
1394 Vector3( s, 0.0f, c ),
1395 Vector3( 0.0f )
1396 );
1397 }
1398
rotationZ(float radians)1399 inline const Transform3 Transform3::rotationZ( float radians )
1400 {
1401 float s, c;
1402 s = sinf( radians );
1403 c = cosf( radians );
1404 return Transform3(
1405 Vector3( c, s, 0.0f ),
1406 Vector3( -s, c, 0.0f ),
1407 Vector3::zAxis( ),
1408 Vector3( 0.0f )
1409 );
1410 }
1411
rotationZYX(const Vector3 & radiansXYZ)1412 inline const Transform3 Transform3::rotationZYX( const Vector3 & radiansXYZ )
1413 {
1414 float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
1415 sX = sinf( radiansXYZ.getX() );
1416 cX = cosf( radiansXYZ.getX() );
1417 sY = sinf( radiansXYZ.getY() );
1418 cY = cosf( radiansXYZ.getY() );
1419 sZ = sinf( radiansXYZ.getZ() );
1420 cZ = cosf( radiansXYZ.getZ() );
1421 tmp0 = ( cZ * sY );
1422 tmp1 = ( sZ * sY );
1423 return Transform3(
1424 Vector3( ( cZ * cY ), ( sZ * cY ), -sY ),
1425 Vector3( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ) ),
1426 Vector3( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ) ),
1427 Vector3( 0.0f )
1428 );
1429 }
1430
rotation(float radians,const Vector3 & unitVec)1431 inline const Transform3 Transform3::rotation( float radians, const Vector3 & unitVec )
1432 {
1433 return Transform3( Matrix3::rotation( radians, unitVec ), Vector3( 0.0f ) );
1434 }
1435
rotation(const Quat & unitQuat)1436 inline const Transform3 Transform3::rotation( const Quat & unitQuat )
1437 {
1438 return Transform3( Matrix3( unitQuat ), Vector3( 0.0f ) );
1439 }
1440
scale(const Vector3 & scaleVec)1441 inline const Transform3 Transform3::scale( const Vector3 & scaleVec )
1442 {
1443 return Transform3(
1444 Vector3( scaleVec.getX(), 0.0f, 0.0f ),
1445 Vector3( 0.0f, scaleVec.getY(), 0.0f ),
1446 Vector3( 0.0f, 0.0f, scaleVec.getZ() ),
1447 Vector3( 0.0f )
1448 );
1449 }
1450
appendScale(const Transform3 & tfrm,const Vector3 & scaleVec)1451 inline const Transform3 appendScale( const Transform3 & tfrm, const Vector3 & scaleVec )
1452 {
1453 return Transform3(
1454 ( tfrm.getCol0() * scaleVec.getX( ) ),
1455 ( tfrm.getCol1() * scaleVec.getY( ) ),
1456 ( tfrm.getCol2() * scaleVec.getZ( ) ),
1457 tfrm.getCol3()
1458 );
1459 }
1460
prependScale(const Vector3 & scaleVec,const Transform3 & tfrm)1461 inline const Transform3 prependScale( const Vector3 & scaleVec, const Transform3 & tfrm )
1462 {
1463 return Transform3(
1464 mulPerElem( tfrm.getCol0(), scaleVec ),
1465 mulPerElem( tfrm.getCol1(), scaleVec ),
1466 mulPerElem( tfrm.getCol2(), scaleVec ),
1467 mulPerElem( tfrm.getCol3(), scaleVec )
1468 );
1469 }
1470
translation(const Vector3 & translateVec)1471 inline const Transform3 Transform3::translation( const Vector3 & translateVec )
1472 {
1473 return Transform3(
1474 Vector3::xAxis( ),
1475 Vector3::yAxis( ),
1476 Vector3::zAxis( ),
1477 translateVec
1478 );
1479 }
1480
select(const Transform3 & tfrm0,const Transform3 & tfrm1,bool select1)1481 inline const Transform3 select( const Transform3 & tfrm0, const Transform3 & tfrm1, bool select1 )
1482 {
1483 return Transform3(
1484 select( tfrm0.getCol0(), tfrm1.getCol0(), select1 ),
1485 select( tfrm0.getCol1(), tfrm1.getCol1(), select1 ),
1486 select( tfrm0.getCol2(), tfrm1.getCol2(), select1 ),
1487 select( tfrm0.getCol3(), tfrm1.getCol3(), select1 )
1488 );
1489 }
1490
1491 #ifdef _VECTORMATH_DEBUG
1492
print(const Transform3 & tfrm)1493 inline void print( const Transform3 & tfrm )
1494 {
1495 print( tfrm.getRow( 0 ) );
1496 print( tfrm.getRow( 1 ) );
1497 print( tfrm.getRow( 2 ) );
1498 }
1499
print(const Transform3 & tfrm,const char * name)1500 inline void print( const Transform3 & tfrm, const char * name )
1501 {
1502 printf("%s:\n", name);
1503 print( tfrm );
1504 }
1505
1506 #endif
1507
Quat(const Matrix3 & tfrm)1508 inline Quat::Quat( const Matrix3 & tfrm )
1509 {
1510 float trace, radicand, scale, xx, yx, zx, xy, yy, zy, xz, yz, zz, tmpx, tmpy, tmpz, tmpw, qx, qy, qz, qw;
1511 int negTrace, ZgtX, ZgtY, YgtX;
1512 int largestXorY, largestYorZ, largestZorX;
1513
1514 xx = tfrm.getCol0().getX();
1515 yx = tfrm.getCol0().getY();
1516 zx = tfrm.getCol0().getZ();
1517 xy = tfrm.getCol1().getX();
1518 yy = tfrm.getCol1().getY();
1519 zy = tfrm.getCol1().getZ();
1520 xz = tfrm.getCol2().getX();
1521 yz = tfrm.getCol2().getY();
1522 zz = tfrm.getCol2().getZ();
1523
1524 trace = ( ( xx + yy ) + zz );
1525
1526 negTrace = ( trace < 0.0f );
1527 ZgtX = zz > xx;
1528 ZgtY = zz > yy;
1529 YgtX = yy > xx;
1530 largestXorY = ( !ZgtX || !ZgtY ) && negTrace;
1531 largestYorZ = ( YgtX || ZgtX ) && negTrace;
1532 largestZorX = ( ZgtY || !YgtX ) && negTrace;
1533
1534 if ( largestXorY )
1535 {
1536 zz = -zz;
1537 xy = -xy;
1538 }
1539 if ( largestYorZ )
1540 {
1541 xx = -xx;
1542 yz = -yz;
1543 }
1544 if ( largestZorX )
1545 {
1546 yy = -yy;
1547 zx = -zx;
1548 }
1549
1550 radicand = ( ( ( xx + yy ) + zz ) + 1.0f );
1551 scale = ( 0.5f * ( 1.0f / sqrtf( radicand ) ) );
1552
1553 tmpx = ( ( zy - yz ) * scale );
1554 tmpy = ( ( xz - zx ) * scale );
1555 tmpz = ( ( yx - xy ) * scale );
1556 tmpw = ( radicand * scale );
1557 qx = tmpx;
1558 qy = tmpy;
1559 qz = tmpz;
1560 qw = tmpw;
1561
1562 if ( largestXorY )
1563 {
1564 qx = tmpw;
1565 qy = tmpz;
1566 qz = tmpy;
1567 qw = tmpx;
1568 }
1569 if ( largestYorZ )
1570 {
1571 tmpx = qx;
1572 tmpz = qz;
1573 qx = qy;
1574 qy = tmpx;
1575 qz = qw;
1576 qw = tmpz;
1577 }
1578
1579 mX = qx;
1580 mY = qy;
1581 mZ = qz;
1582 mW = qw;
1583 }
1584
outer(const Vector3 & tfrm0,const Vector3 & tfrm1)1585 inline const Matrix3 outer( const Vector3 & tfrm0, const Vector3 & tfrm1 )
1586 {
1587 return Matrix3(
1588 ( tfrm0 * tfrm1.getX( ) ),
1589 ( tfrm0 * tfrm1.getY( ) ),
1590 ( tfrm0 * tfrm1.getZ( ) )
1591 );
1592 }
1593
outer(const Vector4 & tfrm0,const Vector4 & tfrm1)1594 inline const Matrix4 outer( const Vector4 & tfrm0, const Vector4 & tfrm1 )
1595 {
1596 return Matrix4(
1597 ( tfrm0 * tfrm1.getX( ) ),
1598 ( tfrm0 * tfrm1.getY( ) ),
1599 ( tfrm0 * tfrm1.getZ( ) ),
1600 ( tfrm0 * tfrm1.getW( ) )
1601 );
1602 }
1603
rowMul(const Vector3 & vec,const Matrix3 & mat)1604 inline const Vector3 rowMul( const Vector3 & vec, const Matrix3 & mat )
1605 {
1606 return Vector3(
1607 ( ( ( vec.getX() * mat.getCol0().getX() ) + ( vec.getY() * mat.getCol0().getY() ) ) + ( vec.getZ() * mat.getCol0().getZ() ) ),
1608 ( ( ( vec.getX() * mat.getCol1().getX() ) + ( vec.getY() * mat.getCol1().getY() ) ) + ( vec.getZ() * mat.getCol1().getZ() ) ),
1609 ( ( ( vec.getX() * mat.getCol2().getX() ) + ( vec.getY() * mat.getCol2().getY() ) ) + ( vec.getZ() * mat.getCol2().getZ() ) )
1610 );
1611 }
1612
crossMatrix(const Vector3 & vec)1613 inline const Matrix3 crossMatrix( const Vector3 & vec )
1614 {
1615 return Matrix3(
1616 Vector3( 0.0f, vec.getZ(), -vec.getY() ),
1617 Vector3( -vec.getZ(), 0.0f, vec.getX() ),
1618 Vector3( vec.getY(), -vec.getX(), 0.0f )
1619 );
1620 }
1621
crossMatrixMul(const Vector3 & vec,const Matrix3 & mat)1622 inline const Matrix3 crossMatrixMul( const Vector3 & vec, const Matrix3 & mat )
1623 {
1624 return Matrix3( cross( vec, mat.getCol0() ), cross( vec, mat.getCol1() ), cross( vec, mat.getCol2() ) );
1625 }
1626
1627 } // namespace Aos
1628 } // namespace Vectormath
1629
1630 #endif
1631