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