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