1/*
2---------------------------------------------------------------------------
3Open Asset Import Library (assimp)
4---------------------------------------------------------------------------
5
6Copyright (c) 2006-2017, assimp team
7
8
9All rights reserved.
10
11Redistribution and use of this software in source and binary forms,
12with or without modification, are permitted provided that the following
13conditions are met:
14
15* Redistributions of source code must retain the above
16 copyright notice, this list of conditions and the
17 following disclaimer.
18
19* Redistributions in binary form must reproduce the above
20 copyright notice, this list of conditions and the
21 following disclaimer in the documentation and/or other
22 materials provided with the distribution.
23
24* Neither the name of the assimp team, nor the names of its
25 contributors may be used to endorse or promote products
26 derived from this software without specific prior
27 written permission of the assimp team.
28
29THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
30"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
31LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
32A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
33OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
34SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
35LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
36DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
37THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
38(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
39OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
40---------------------------------------------------------------------------
41*/
42
43/** @file matrix4x4.inl
44 * @brief Inline implementation of the 4x4 matrix operators
45 */
46#pragma once
47#ifndef AI_MATRIX4X4_INL_INC
48#define AI_MATRIX4X4_INL_INC
49
50#ifdef __cplusplus
51
52#include "matrix4x4.h"
53#include "matrix3x3.h"
54#include "quaternion.h"
55
56#include <algorithm>
57#include <limits>
58#include <cmath>
59
60// ----------------------------------------------------------------------------------------
61template <typename TReal>
62aiMatrix4x4t<TReal> ::aiMatrix4x4t () :
63 a1(1.0f), a2(), a3(), a4(),
64 b1(), b2(1.0f), b3(), b4(),
65 c1(), c2(), c3(1.0f), c4(),
66 d1(), d2(), d3(), d4(1.0f)
67{
68
69}
70
71// ----------------------------------------------------------------------------------------
72template <typename TReal>
73aiMatrix4x4t<TReal> ::aiMatrix4x4t (TReal _a1, TReal _a2, TReal _a3, TReal _a4,
74 TReal _b1, TReal _b2, TReal _b3, TReal _b4,
75 TReal _c1, TReal _c2, TReal _c3, TReal _c4,
76 TReal _d1, TReal _d2, TReal _d3, TReal _d4) :
77 a1(_a1), a2(_a2), a3(_a3), a4(_a4),
78 b1(_b1), b2(_b2), b3(_b3), b4(_b4),
79 c1(_c1), c2(_c2), c3(_c3), c4(_c4),
80 d1(_d1), d2(_d2), d3(_d3), d4(_d4)
81{
82
83}
84
85// ------------------------------------------------------------------------------------------------
86template <typename TReal>
87template <typename TOther>
88aiMatrix4x4t<TReal>::operator aiMatrix4x4t<TOther> () const
89{
90 return aiMatrix4x4t<TOther>(static_cast<TOther>(a1),static_cast<TOther>(a2),static_cast<TOther>(a3),static_cast<TOther>(a4),
91 static_cast<TOther>(b1),static_cast<TOther>(b2),static_cast<TOther>(b3),static_cast<TOther>(b4),
92 static_cast<TOther>(c1),static_cast<TOther>(c2),static_cast<TOther>(c3),static_cast<TOther>(c4),
93 static_cast<TOther>(d1),static_cast<TOther>(d2),static_cast<TOther>(d3),static_cast<TOther>(d4));
94}
95
96
97// ----------------------------------------------------------------------------------------
98template <typename TReal>
99inline aiMatrix4x4t<TReal>::aiMatrix4x4t (const aiMatrix3x3t<TReal>& m)
100{
101 a1 = m.a1; a2 = m.a2; a3 = m.a3; a4 = static_cast<TReal>(0.0);
102 b1 = m.b1; b2 = m.b2; b3 = m.b3; b4 = static_cast<TReal>(0.0);
103 c1 = m.c1; c2 = m.c2; c3 = m.c3; c4 = static_cast<TReal>(0.0);
104 d1 = static_cast<TReal>(0.0); d2 = static_cast<TReal>(0.0); d3 = static_cast<TReal>(0.0); d4 = static_cast<TReal>(1.0);
105}
106
107// ----------------------------------------------------------------------------------------
108template <typename TReal>
109inline aiMatrix4x4t<TReal>::aiMatrix4x4t (const aiVector3t<TReal>& scaling, const aiQuaterniont<TReal>& rotation, const aiVector3t<TReal>& position)
110{
111 // build a 3x3 rotation matrix
112 aiMatrix3x3t<TReal> m = rotation.GetMatrix();
113
114 a1 = m.a1 * scaling.x;
115 a2 = m.a2 * scaling.x;
116 a3 = m.a3 * scaling.x;
117 a4 = position.x;
118
119 b1 = m.b1 * scaling.y;
120 b2 = m.b2 * scaling.y;
121 b3 = m.b3 * scaling.y;
122 b4 = position.y;
123
124 c1 = m.c1 * scaling.z;
125 c2 = m.c2 * scaling.z;
126 c3 = m.c3 * scaling.z;
127 c4= position.z;
128
129 d1 = static_cast<TReal>(0.0);
130 d2 = static_cast<TReal>(0.0);
131 d3 = static_cast<TReal>(0.0);
132 d4 = static_cast<TReal>(1.0);
133}
134
135// ----------------------------------------------------------------------------------------
136template <typename TReal>
137inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::operator *= (const aiMatrix4x4t<TReal>& m)
138{
139 *this = aiMatrix4x4t<TReal>(
140 m.a1 * a1 + m.b1 * a2 + m.c1 * a3 + m.d1 * a4,
141 m.a2 * a1 + m.b2 * a2 + m.c2 * a3 + m.d2 * a4,
142 m.a3 * a1 + m.b3 * a2 + m.c3 * a3 + m.d3 * a4,
143 m.a4 * a1 + m.b4 * a2 + m.c4 * a3 + m.d4 * a4,
144 m.a1 * b1 + m.b1 * b2 + m.c1 * b3 + m.d1 * b4,
145 m.a2 * b1 + m.b2 * b2 + m.c2 * b3 + m.d2 * b4,
146 m.a3 * b1 + m.b3 * b2 + m.c3 * b3 + m.d3 * b4,
147 m.a4 * b1 + m.b4 * b2 + m.c4 * b3 + m.d4 * b4,
148 m.a1 * c1 + m.b1 * c2 + m.c1 * c3 + m.d1 * c4,
149 m.a2 * c1 + m.b2 * c2 + m.c2 * c3 + m.d2 * c4,
150 m.a3 * c1 + m.b3 * c2 + m.c3 * c3 + m.d3 * c4,
151 m.a4 * c1 + m.b4 * c2 + m.c4 * c3 + m.d4 * c4,
152 m.a1 * d1 + m.b1 * d2 + m.c1 * d3 + m.d1 * d4,
153 m.a2 * d1 + m.b2 * d2 + m.c2 * d3 + m.d2 * d4,
154 m.a3 * d1 + m.b3 * d2 + m.c3 * d3 + m.d3 * d4,
155 m.a4 * d1 + m.b4 * d2 + m.c4 * d3 + m.d4 * d4);
156 return *this;
157}
158
159// ----------------------------------------------------------------------------------------
160template <typename TReal>
161inline aiMatrix4x4t<TReal> aiMatrix4x4t<TReal>::operator* (const TReal& aFloat) const
162{
163 aiMatrix4x4t<TReal> temp(
164 a1 * aFloat,
165 a2 * aFloat,
166 a3 * aFloat,
167 a4 * aFloat,
168 b1 * aFloat,
169 b2 * aFloat,
170 b3 * aFloat,
171 b4 * aFloat,
172 c1 * aFloat,
173 c2 * aFloat,
174 c3 * aFloat,
175 c4 * aFloat,
176 d1 * aFloat,
177 d2 * aFloat,
178 d3 * aFloat,
179 d4 * aFloat);
180 return temp;
181}
182
183// ----------------------------------------------------------------------------------------
184template <typename TReal>
185inline aiMatrix4x4t<TReal> aiMatrix4x4t<TReal>::operator+ (const aiMatrix4x4t<TReal>& m) const
186{
187 aiMatrix4x4t<TReal> temp(
188 m.a1 + a1,
189 m.a2 + a2,
190 m.a3 + a3,
191 m.a4 + a4,
192 m.b1 + b1,
193 m.b2 + b2,
194 m.b3 + b3,
195 m.b4 + b4,
196 m.c1 + c1,
197 m.c2 + c2,
198 m.c3 + c3,
199 m.c4 + c4,
200 m.d1 + d1,
201 m.d2 + d2,
202 m.d3 + d3,
203 m.d4 + d4);
204 return temp;
205}
206
207// ----------------------------------------------------------------------------------------
208template <typename TReal>
209inline aiMatrix4x4t<TReal> aiMatrix4x4t<TReal>::operator* (const aiMatrix4x4t<TReal>& m) const
210{
211 aiMatrix4x4t<TReal> temp( *this);
212 temp *= m;
213 return temp;
214}
215
216
217// ----------------------------------------------------------------------------------------
218template <typename TReal>
219inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Transpose()
220{
221 // (TReal&) don't remove, GCC complains cause of packed fields
222 std::swap( (TReal&)b1, (TReal&)a2);
223 std::swap( (TReal&)c1, (TReal&)a3);
224 std::swap( (TReal&)c2, (TReal&)b3);
225 std::swap( (TReal&)d1, (TReal&)a4);
226 std::swap( (TReal&)d2, (TReal&)b4);
227 std::swap( (TReal&)d3, (TReal&)c4);
228 return *this;
229}
230
231
232// ----------------------------------------------------------------------------------------
233template <typename TReal>
234inline TReal aiMatrix4x4t<TReal>::Determinant() const
235{
236 return a1*b2*c3*d4 - a1*b2*c4*d3 + a1*b3*c4*d2 - a1*b3*c2*d4
237 + a1*b4*c2*d3 - a1*b4*c3*d2 - a2*b3*c4*d1 + a2*b3*c1*d4
238 - a2*b4*c1*d3 + a2*b4*c3*d1 - a2*b1*c3*d4 + a2*b1*c4*d3
239 + a3*b4*c1*d2 - a3*b4*c2*d1 + a3*b1*c2*d4 - a3*b1*c4*d2
240 + a3*b2*c4*d1 - a3*b2*c1*d4 - a4*b1*c2*d3 + a4*b1*c3*d2
241 - a4*b2*c3*d1 + a4*b2*c1*d3 - a4*b3*c1*d2 + a4*b3*c2*d1;
242}
243
244// ----------------------------------------------------------------------------------------
245template <typename TReal>
246inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Inverse()
247{
248 // Compute the reciprocal determinant
249 const TReal det = Determinant();
250 if(det == static_cast<TReal>(0.0))
251 {
252 // Matrix not invertible. Setting all elements to nan is not really
253 // correct in a mathematical sense but it is easy to debug for the
254 // programmer.
255 const TReal nan = std::numeric_limits<TReal>::quiet_NaN();
256 *this = aiMatrix4x4t<TReal>(
257 nan,nan,nan,nan,
258 nan,nan,nan,nan,
259 nan,nan,nan,nan,
260 nan,nan,nan,nan);
261
262 return *this;
263 }
264
265 const TReal invdet = static_cast<TReal>(1.0) / det;
266
267 aiMatrix4x4t<TReal> res;
268 res.a1 = invdet * (b2 * (c3 * d4 - c4 * d3) + b3 * (c4 * d2 - c2 * d4) + b4 * (c2 * d3 - c3 * d2));
269 res.a2 = -invdet * (a2 * (c3 * d4 - c4 * d3) + a3 * (c4 * d2 - c2 * d4) + a4 * (c2 * d3 - c3 * d2));
270 res.a3 = invdet * (a2 * (b3 * d4 - b4 * d3) + a3 * (b4 * d2 - b2 * d4) + a4 * (b2 * d3 - b3 * d2));
271 res.a4 = -invdet * (a2 * (b3 * c4 - b4 * c3) + a3 * (b4 * c2 - b2 * c4) + a4 * (b2 * c3 - b3 * c2));
272 res.b1 = -invdet * (b1 * (c3 * d4 - c4 * d3) + b3 * (c4 * d1 - c1 * d4) + b4 * (c1 * d3 - c3 * d1));
273 res.b2 = invdet * (a1 * (c3 * d4 - c4 * d3) + a3 * (c4 * d1 - c1 * d4) + a4 * (c1 * d3 - c3 * d1));
274 res.b3 = -invdet * (a1 * (b3 * d4 - b4 * d3) + a3 * (b4 * d1 - b1 * d4) + a4 * (b1 * d3 - b3 * d1));
275 res.b4 = invdet * (a1 * (b3 * c4 - b4 * c3) + a3 * (b4 * c1 - b1 * c4) + a4 * (b1 * c3 - b3 * c1));
276 res.c1 = invdet * (b1 * (c2 * d4 - c4 * d2) + b2 * (c4 * d1 - c1 * d4) + b4 * (c1 * d2 - c2 * d1));
277 res.c2 = -invdet * (a1 * (c2 * d4 - c4 * d2) + a2 * (c4 * d1 - c1 * d4) + a4 * (c1 * d2 - c2 * d1));
278 res.c3 = invdet * (a1 * (b2 * d4 - b4 * d2) + a2 * (b4 * d1 - b1 * d4) + a4 * (b1 * d2 - b2 * d1));
279 res.c4 = -invdet * (a1 * (b2 * c4 - b4 * c2) + a2 * (b4 * c1 - b1 * c4) + a4 * (b1 * c2 - b2 * c1));
280 res.d1 = -invdet * (b1 * (c2 * d3 - c3 * d2) + b2 * (c3 * d1 - c1 * d3) + b3 * (c1 * d2 - c2 * d1));
281 res.d2 = invdet * (a1 * (c2 * d3 - c3 * d2) + a2 * (c3 * d1 - c1 * d3) + a3 * (c1 * d2 - c2 * d1));
282 res.d3 = -invdet * (a1 * (b2 * d3 - b3 * d2) + a2 * (b3 * d1 - b1 * d3) + a3 * (b1 * d2 - b2 * d1));
283 res.d4 = invdet * (a1 * (b2 * c3 - b3 * c2) + a2 * (b3 * c1 - b1 * c3) + a3 * (b1 * c2 - b2 * c1));
284 *this = res;
285
286 return *this;
287}
288
289// ----------------------------------------------------------------------------------------
290template <typename TReal>
291inline TReal* aiMatrix4x4t<TReal>::operator[](unsigned int p_iIndex) {
292 if (p_iIndex > 3) {
293 return NULL;
294 }
295 switch ( p_iIndex ) {
296 case 0:
297 return &a1;
298 case 1:
299 return &b1;
300 case 2:
301 return &c1;
302 case 3:
303 return &d1;
304 default:
305 break;
306 }
307 return &a1;
308}
309
310// ----------------------------------------------------------------------------------------
311template <typename TReal>
312inline const TReal* aiMatrix4x4t<TReal>::operator[](unsigned int p_iIndex) const {
313 if (p_iIndex > 3) {
314 return NULL;
315 }
316
317 switch ( p_iIndex ) {
318 case 0:
319 return &a1;
320 case 1:
321 return &b1;
322 case 2:
323 return &c1;
324 case 3:
325 return &d1;
326 default:
327 break;
328 }
329 return &a1;
330}
331
332// ----------------------------------------------------------------------------------------
333template <typename TReal>
334inline bool aiMatrix4x4t<TReal>::operator== (const aiMatrix4x4t<TReal>& m) const
335{
336 return (a1 == m.a1 && a2 == m.a2 && a3 == m.a3 && a4 == m.a4 &&
337 b1 == m.b1 && b2 == m.b2 && b3 == m.b3 && b4 == m.b4 &&
338 c1 == m.c1 && c2 == m.c2 && c3 == m.c3 && c4 == m.c4 &&
339 d1 == m.d1 && d2 == m.d2 && d3 == m.d3 && d4 == m.d4);
340}
341
342// ----------------------------------------------------------------------------------------
343template <typename TReal>
344inline bool aiMatrix4x4t<TReal>::operator!= (const aiMatrix4x4t<TReal>& m) const
345{
346 return !(*this == m);
347}
348
349// ---------------------------------------------------------------------------
350template<typename TReal>
351inline bool aiMatrix4x4t<TReal>::Equal(const aiMatrix4x4t<TReal>& m, TReal epsilon) const {
352 return
353 std::abs(a1 - m.a1) <= epsilon &&
354 std::abs(a2 - m.a2) <= epsilon &&
355 std::abs(a3 - m.a3) <= epsilon &&
356 std::abs(a4 - m.a4) <= epsilon &&
357 std::abs(b1 - m.b1) <= epsilon &&
358 std::abs(b2 - m.b2) <= epsilon &&
359 std::abs(b3 - m.b3) <= epsilon &&
360 std::abs(b4 - m.b4) <= epsilon &&
361 std::abs(c1 - m.c1) <= epsilon &&
362 std::abs(c2 - m.c2) <= epsilon &&
363 std::abs(c3 - m.c3) <= epsilon &&
364 std::abs(c4 - m.c4) <= epsilon &&
365 std::abs(d1 - m.d1) <= epsilon &&
366 std::abs(d2 - m.d2) <= epsilon &&
367 std::abs(d3 - m.d3) <= epsilon &&
368 std::abs(d4 - m.d4) <= epsilon;
369}
370
371// ----------------------------------------------------------------------------------------
372
373#define ASSIMP_MATRIX4_4_DECOMPOSE_PART \
374 const aiMatrix4x4t<TReal>& _this = *this;/* Create alias for conveniance. */ \
375 \
376 /* extract translation */ \
377 pPosition.x = _this[0][3]; \
378 pPosition.y = _this[1][3]; \
379 pPosition.z = _this[2][3]; \
380 \
381 /* extract the columns of the matrix. */ \
382 aiVector3t<TReal> vCols[3] = { \
383 aiVector3t<TReal>(_this[0][0],_this[1][0],_this[2][0]), \
384 aiVector3t<TReal>(_this[0][1],_this[1][1],_this[2][1]), \
385 aiVector3t<TReal>(_this[0][2],_this[1][2],_this[2][2]) \
386 }; \
387 \
388 /* extract the scaling factors */ \
389 pScaling.x = vCols[0].Length(); \
390 pScaling.y = vCols[1].Length(); \
391 pScaling.z = vCols[2].Length(); \
392 \
393 /* and the sign of the scaling */ \
394 if (Determinant() < 0) pScaling = -pScaling; \
395 \
396 /* and remove all scaling from the matrix */ \
397 if(pScaling.x) vCols[0] /= pScaling.x; \
398 if(pScaling.y) vCols[1] /= pScaling.y; \
399 if(pScaling.z) vCols[2] /= pScaling.z; \
400 \
401 do {} while(false)
402
403
404
405
406template <typename TReal>
407inline void aiMatrix4x4t<TReal>::Decompose (aiVector3t<TReal>& pScaling, aiQuaterniont<TReal>& pRotation,
408 aiVector3t<TReal>& pPosition) const
409{
410 ASSIMP_MATRIX4_4_DECOMPOSE_PART;
411
412 // build a 3x3 rotation matrix
413 aiMatrix3x3t<TReal> m(vCols[0].x,vCols[1].x,vCols[2].x,
414 vCols[0].y,vCols[1].y,vCols[2].y,
415 vCols[0].z,vCols[1].z,vCols[2].z);
416
417 // and generate the rotation quaternion from it
418 pRotation = aiQuaterniont<TReal>(m);
419}
420
421template <typename TReal>
422inline void aiMatrix4x4t<TReal>::Decompose(aiVector3t<TReal>& pScaling, aiVector3t<TReal>& pRotation, aiVector3t<TReal>& pPosition) const
423{
424 ASSIMP_MATRIX4_4_DECOMPOSE_PART;
425
426 /*
427 | CE -CF D 0 |
428 M = | BDE+AF -BDF+AE -BC 0 |
429 | -ADE+BF -ADF+BE AC 0 |
430 | 0 0 0 1 |
431
432 A = cos(angle_x), B = sin(angle_x);
433 C = cos(angle_y), D = sin(angle_y);
434 E = cos(angle_z), F = sin(angle_z);
435 */
436
437 // Use a small epsilon to solve floating-point inaccuracies
438 const TReal epsilon = 10e-3f;
439
440 pRotation.y = std::asin(vCols[2].x);// D. Angle around oY.
441
442 TReal C = std::cos(pRotation.y);
443
444 if(std::fabs(C) > epsilon)
445 {
446 // Finding angle around oX.
447 TReal tan_x = vCols[2].z / C;// A
448 TReal tan_y = -vCols[2].y / C;// B
449
450 pRotation.x = std::atan2(tan_y, tan_x);
451 // Finding angle around oZ.
452 tan_x = vCols[0].x / C;// E
453 tan_y = -vCols[1].x / C;// F
454 pRotation.z = std::atan2(tan_y, tan_x);
455 }
456 else
457 {// oY is fixed.
458 pRotation.x = 0;// Set angle around oX to 0. => A == 1, B == 0, C == 0, D == 1.
459
460 // And finding angle around oZ.
461 TReal tan_x = vCols[1].y;// -BDF+AE => E
462 TReal tan_y = vCols[0].y;// BDE+AF => F
463
464 pRotation.z = std::atan2(tan_y, tan_x);
465 }
466}
467
468#undef ASSIMP_MATRIX4_4_DECOMPOSE_PART
469
470template <typename TReal>
471inline void aiMatrix4x4t<TReal>::Decompose(aiVector3t<TReal>& pScaling, aiVector3t<TReal>& pRotationAxis, TReal& pRotationAngle,
472 aiVector3t<TReal>& pPosition) const
473{
474aiQuaterniont<TReal> pRotation;
475
476 Decompose(pScaling, pRotation, pPosition);
477 pRotation.Normalize();
478
479 TReal angle_cos = pRotation.w;
480 TReal angle_sin = std::sqrt(1.0f - angle_cos * angle_cos);
481
482 pRotationAngle = std::acos(angle_cos) * 2;
483
484 // Use a small epsilon to solve floating-point inaccuracies
485 const TReal epsilon = 10e-3f;
486
487 if(std::fabs(angle_sin) < epsilon) angle_sin = 1;
488
489 pRotationAxis.x = pRotation.x / angle_sin;
490 pRotationAxis.y = pRotation.y / angle_sin;
491 pRotationAxis.z = pRotation.z / angle_sin;
492}
493
494// ----------------------------------------------------------------------------------------
495template <typename TReal>
496inline void aiMatrix4x4t<TReal>::DecomposeNoScaling (aiQuaterniont<TReal>& rotation,
497 aiVector3t<TReal>& position) const
498{
499 const aiMatrix4x4t<TReal>& _this = *this;
500
501 // extract translation
502 position.x = _this[0][3];
503 position.y = _this[1][3];
504 position.z = _this[2][3];
505
506 // extract rotation
507 rotation = aiQuaterniont<TReal>((aiMatrix3x3t<TReal>)_this);
508}
509
510// ----------------------------------------------------------------------------------------
511template <typename TReal>
512inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromEulerAnglesXYZ(const aiVector3t<TReal>& blubb)
513{
514 return FromEulerAnglesXYZ(blubb.x,blubb.y,blubb.z);
515}
516
517// ----------------------------------------------------------------------------------------
518template <typename TReal>
519inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromEulerAnglesXYZ(TReal x, TReal y, TReal z)
520{
521 aiMatrix4x4t<TReal>& _this = *this;
522
523 TReal cr = std::cos( x );
524 TReal sr = std::sin( x );
525 TReal cp = std::cos( y );
526 TReal sp = std::sin( y );
527 TReal cy = std::cos( z );
528 TReal sy = std::sin( z );
529
530 _this.a1 = cp*cy ;
531 _this.a2 = cp*sy;
532 _this.a3 = -sp ;
533
534 TReal srsp = sr*sp;
535 TReal crsp = cr*sp;
536
537 _this.b1 = srsp*cy-cr*sy ;
538 _this.b2 = srsp*sy+cr*cy ;
539 _this.b3 = sr*cp ;
540
541 _this.c1 = crsp*cy+sr*sy ;
542 _this.c2 = crsp*sy-sr*cy ;
543 _this.c3 = cr*cp ;
544
545 return *this;
546}
547
548// ----------------------------------------------------------------------------------------
549template <typename TReal>
550inline bool aiMatrix4x4t<TReal>::IsIdentity() const
551{
552 // Use a small epsilon to solve floating-point inaccuracies
553 const static TReal epsilon = 10e-3f;
554
555 return (a2 <= epsilon && a2 >= -epsilon &&
556 a3 <= epsilon && a3 >= -epsilon &&
557 a4 <= epsilon && a4 >= -epsilon &&
558 b1 <= epsilon && b1 >= -epsilon &&
559 b3 <= epsilon && b3 >= -epsilon &&
560 b4 <= epsilon && b4 >= -epsilon &&
561 c1 <= epsilon && c1 >= -epsilon &&
562 c2 <= epsilon && c2 >= -epsilon &&
563 c4 <= epsilon && c4 >= -epsilon &&
564 d1 <= epsilon && d1 >= -epsilon &&
565 d2 <= epsilon && d2 >= -epsilon &&
566 d3 <= epsilon && d3 >= -epsilon &&
567 a1 <= 1.f+epsilon && a1 >= 1.f-epsilon &&
568 b2 <= 1.f+epsilon && b2 >= 1.f-epsilon &&
569 c3 <= 1.f+epsilon && c3 >= 1.f-epsilon &&
570 d4 <= 1.f+epsilon && d4 >= 1.f-epsilon);
571}
572
573// ----------------------------------------------------------------------------------------
574template <typename TReal>
575inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationX(TReal a, aiMatrix4x4t<TReal>& out)
576{
577 /*
578 | 1 0 0 0 |
579 M = | 0 cos(A) -sin(A) 0 |
580 | 0 sin(A) cos(A) 0 |
581 | 0 0 0 1 | */
582 out = aiMatrix4x4t<TReal>();
583 out.b2 = out.c3 = std::cos(a);
584 out.b3 = -(out.c2 = std::sin(a));
585 return out;
586}
587
588// ----------------------------------------------------------------------------------------
589template <typename TReal>
590inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationY(TReal a, aiMatrix4x4t<TReal>& out)
591{
592 /*
593 | cos(A) 0 sin(A) 0 |
594 M = | 0 1 0 0 |
595 | -sin(A) 0 cos(A) 0 |
596 | 0 0 0 1 |
597 */
598 out = aiMatrix4x4t<TReal>();
599 out.a1 = out.c3 = std::cos(a);
600 out.c1 = -(out.a3 = std::sin(a));
601 return out;
602}
603
604// ----------------------------------------------------------------------------------------
605template <typename TReal>
606inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationZ(TReal a, aiMatrix4x4t<TReal>& out)
607{
608 /*
609 | cos(A) -sin(A) 0 0 |
610 M = | sin(A) cos(A) 0 0 |
611 | 0 0 1 0 |
612 | 0 0 0 1 | */
613 out = aiMatrix4x4t<TReal>();
614 out.a1 = out.b2 = std::cos(a);
615 out.a2 = -(out.b1 = std::sin(a));
616 return out;
617}
618
619// ----------------------------------------------------------------------------------------
620// Returns a rotation matrix for a rotation around an arbitrary axis.
621template <typename TReal>
622inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Rotation( TReal a, const aiVector3t<TReal>& axis, aiMatrix4x4t<TReal>& out)
623{
624 TReal c = std::cos( a), s = std::sin( a), t = 1 - c;
625 TReal x = axis.x, y = axis.y, z = axis.z;
626
627 // Many thanks to MathWorld and Wikipedia
628 out.a1 = t*x*x + c; out.a2 = t*x*y - s*z; out.a3 = t*x*z + s*y;
629 out.b1 = t*x*y + s*z; out.b2 = t*y*y + c; out.b3 = t*y*z - s*x;
630 out.c1 = t*x*z - s*y; out.c2 = t*y*z + s*x; out.c3 = t*z*z + c;
631 out.a4 = out.b4 = out.c4 = static_cast<TReal>(0.0);
632 out.d1 = out.d2 = out.d3 = static_cast<TReal>(0.0);
633 out.d4 = static_cast<TReal>(1.0);
634
635 return out;
636}
637
638// ----------------------------------------------------------------------------------------
639template <typename TReal>
640inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Translation( const aiVector3t<TReal>& v, aiMatrix4x4t<TReal>& out)
641{
642 out = aiMatrix4x4t<TReal>();
643 out.a4 = v.x;
644 out.b4 = v.y;
645 out.c4 = v.z;
646 return out;
647}
648
649// ----------------------------------------------------------------------------------------
650template <typename TReal>
651inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Scaling( const aiVector3t<TReal>& v, aiMatrix4x4t<TReal>& out)
652{
653 out = aiMatrix4x4t<TReal>();
654 out.a1 = v.x;
655 out.b2 = v.y;
656 out.c3 = v.z;
657 return out;
658}
659
660// ----------------------------------------------------------------------------------------
661/** A function for creating a rotation matrix that rotates a vector called
662 * "from" into another vector called "to".
663 * Input : from[3], to[3] which both must be *normalized* non-zero vectors
664 * Output: mtx[3][3] -- a 3x3 matrix in colum-major form
665 * Authors: Tomas Möller, John Hughes
666 * "Efficiently Building a Matrix to Rotate One Vector to Another"
667 * Journal of Graphics Tools, 4(4):1-4, 1999
668 */
669// ----------------------------------------------------------------------------------------
670template <typename TReal>
671inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromToMatrix(const aiVector3t<TReal>& from,
672 const aiVector3t<TReal>& to, aiMatrix4x4t<TReal>& mtx)
673{
674 aiMatrix3x3t<TReal> m3;
675 aiMatrix3x3t<TReal>::FromToMatrix(from,to,m3);
676 mtx = aiMatrix4x4t<TReal>(m3);
677 return mtx;
678}
679
680#endif // __cplusplus
681#endif // AI_MATRIX4X4_INL_INC
682