1 | /* |
2 | --------------------------------------------------------------------------- |
3 | Open Asset Import Library (assimp) |
4 | --------------------------------------------------------------------------- |
5 | |
6 | Copyright (c) 2006-2019, assimp team |
7 | |
8 | All rights reserved. |
9 | |
10 | Redistribution and use of this software in source and binary forms, |
11 | with or without modification, are permitted provided that the following |
12 | conditions are met: |
13 | |
14 | * Redistributions of source code must retain the above |
15 | copyright notice, this list of conditions and the |
16 | following disclaimer. |
17 | |
18 | * Redistributions in binary form must reproduce the above |
19 | copyright notice, this list of conditions and the |
20 | following disclaimer in the documentation and/or other |
21 | materials provided with the distribution. |
22 | |
23 | * Neither the name of the assimp team, nor the names of its |
24 | contributors may be used to endorse or promote products |
25 | derived from this software without specific prior |
26 | written permission of the assimp team. |
27 | |
28 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
29 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
30 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
31 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |
32 | OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
33 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
34 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |
35 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |
36 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
37 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
38 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
39 | --------------------------------------------------------------------------- |
40 | */ |
41 | |
42 | /** @file matrix4x4.inl |
43 | * @brief Inline implementation of the 4x4 matrix operators |
44 | */ |
45 | #pragma once |
46 | #ifndef AI_MATRIX4X4_INL_INC |
47 | #define AI_MATRIX4X4_INL_INC |
48 | |
49 | #ifdef __cplusplus |
50 | |
51 | #include "matrix4x4.h" |
52 | #include "matrix3x3.h" |
53 | #include "quaternion.h" |
54 | #include "MathFunctions.h" |
55 | |
56 | #include <algorithm> |
57 | #include <limits> |
58 | #include <cmath> |
59 | |
60 | // ---------------------------------------------------------------------------------------- |
61 | template <typename TReal> |
62 | aiMatrix4x4t<TReal>::aiMatrix4x4t() AI_NO_EXCEPT : |
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 | // ---------------------------------------------------------------------------------------- |
72 | template <typename TReal> |
73 | aiMatrix4x4t<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 | // ------------------------------------------------------------------------------------------------ |
86 | template <typename TReal> |
87 | template <typename TOther> |
88 | aiMatrix4x4t<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 | // ---------------------------------------------------------------------------------------- |
98 | template <typename TReal> |
99 | inline 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 | // ---------------------------------------------------------------------------------------- |
108 | template <typename TReal> |
109 | inline 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 | // ---------------------------------------------------------------------------------------- |
136 | template <typename TReal> |
137 | inline 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 | // ---------------------------------------------------------------------------------------- |
160 | template <typename TReal> |
161 | inline 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 | // ---------------------------------------------------------------------------------------- |
184 | template <typename TReal> |
185 | inline 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 | // ---------------------------------------------------------------------------------------- |
208 | template <typename TReal> |
209 | inline 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 | // ---------------------------------------------------------------------------------------- |
218 | template <typename TReal> |
219 | inline 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 | // ---------------------------------------------------------------------------------------- |
233 | template <typename TReal> |
234 | inline 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 | // ---------------------------------------------------------------------------------------- |
245 | template <typename TReal> |
246 | inline 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 | // ---------------------------------------------------------------------------------------- |
290 | template <typename TReal> |
291 | inline 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 | // ---------------------------------------------------------------------------------------- |
311 | template <typename TReal> |
312 | inline 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 | // ---------------------------------------------------------------------------------------- |
333 | template <typename TReal> |
334 | inline 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 | // ---------------------------------------------------------------------------------------- |
343 | template <typename TReal> |
344 | inline bool aiMatrix4x4t<TReal>::operator!= (const aiMatrix4x4t<TReal>& m) const |
345 | { |
346 | return !(*this == m); |
347 | } |
348 | |
349 | // --------------------------------------------------------------------------- |
350 | template<typename TReal> |
351 | inline 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 | |
406 | template <typename TReal> |
407 | inline 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 | |
421 | template <typename TReal> |
422 | inline |
423 | void aiMatrix4x4t<TReal>::Decompose(aiVector3t<TReal>& pScaling, aiVector3t<TReal>& pRotation, aiVector3t<TReal>& pPosition) const { |
424 | ASSIMP_MATRIX4_4_DECOMPOSE_PART; |
425 | |
426 | /* |
427 | assuming a right-handed coordinate system |
428 | and post-multiplication of column vectors, |
429 | the rotation matrix for an euler XYZ rotation is M = Rz * Ry * Rx. |
430 | combining gives: |
431 | |
432 | | CE BDE-AF ADE+BF 0 | |
433 | M = | CF BDF+AE ADF-BE 0 | |
434 | | -D CB AC 0 | |
435 | | 0 0 0 1 | |
436 | |
437 | where |
438 | A = cos(angle_x), B = sin(angle_x); |
439 | C = cos(angle_y), D = sin(angle_y); |
440 | E = cos(angle_z), F = sin(angle_z); |
441 | */ |
442 | |
443 | // Use a small epsilon to solve floating-point inaccuracies |
444 | const TReal epsilon = Assimp::Math::getEpsilon<TReal>(); |
445 | |
446 | pRotation.y = std::asin(-vCols[0].z);// D. Angle around oY. |
447 | |
448 | TReal C = std::cos(pRotation.y); |
449 | |
450 | if(std::fabs(C) > epsilon) |
451 | { |
452 | // Finding angle around oX. |
453 | TReal tan_x = vCols[2].z / C;// A |
454 | TReal tan_y = vCols[1].z / C;// B |
455 | |
456 | pRotation.x = std::atan2(tan_y, tan_x); |
457 | // Finding angle around oZ. |
458 | tan_x = vCols[0].x / C;// E |
459 | tan_y = vCols[0].y / C;// F |
460 | pRotation.z = std::atan2(tan_y, tan_x); |
461 | } |
462 | else |
463 | {// oY is fixed. |
464 | pRotation.x = 0;// Set angle around oX to 0. => A == 1, B == 0, C == 0, D == 1. |
465 | |
466 | // And finding angle around oZ. |
467 | TReal tan_x = vCols[1].y;// BDF+AE => E |
468 | TReal tan_y = -vCols[1].x;// BDE-AF => F |
469 | |
470 | pRotation.z = std::atan2(tan_y, tan_x); |
471 | } |
472 | } |
473 | |
474 | #undef ASSIMP_MATRIX4_4_DECOMPOSE_PART |
475 | |
476 | template <typename TReal> |
477 | inline void aiMatrix4x4t<TReal>::Decompose(aiVector3t<TReal>& pScaling, aiVector3t<TReal>& pRotationAxis, TReal& pRotationAngle, |
478 | aiVector3t<TReal>& pPosition) const |
479 | { |
480 | aiQuaterniont<TReal> pRotation; |
481 | |
482 | Decompose(pScaling, pRotation, pPosition); |
483 | pRotation.Normalize(); |
484 | |
485 | TReal angle_cos = pRotation.w; |
486 | TReal angle_sin = std::sqrt(1.0f - angle_cos * angle_cos); |
487 | |
488 | pRotationAngle = std::acos(angle_cos) * 2; |
489 | |
490 | // Use a small epsilon to solve floating-point inaccuracies |
491 | const TReal epsilon = 10e-3f; |
492 | |
493 | if(std::fabs(angle_sin) < epsilon) angle_sin = 1; |
494 | |
495 | pRotationAxis.x = pRotation.x / angle_sin; |
496 | pRotationAxis.y = pRotation.y / angle_sin; |
497 | pRotationAxis.z = pRotation.z / angle_sin; |
498 | } |
499 | |
500 | // ---------------------------------------------------------------------------------------- |
501 | template <typename TReal> |
502 | inline void aiMatrix4x4t<TReal>::DecomposeNoScaling (aiQuaterniont<TReal>& rotation, |
503 | aiVector3t<TReal>& position) const |
504 | { |
505 | const aiMatrix4x4t<TReal>& _this = *this; |
506 | |
507 | // extract translation |
508 | position.x = _this[0][3]; |
509 | position.y = _this[1][3]; |
510 | position.z = _this[2][3]; |
511 | |
512 | // extract rotation |
513 | rotation = aiQuaterniont<TReal>((aiMatrix3x3t<TReal>)_this); |
514 | } |
515 | |
516 | // ---------------------------------------------------------------------------------------- |
517 | template <typename TReal> |
518 | inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromEulerAnglesXYZ(const aiVector3t<TReal>& blubb) |
519 | { |
520 | return FromEulerAnglesXYZ(blubb.x,blubb.y,blubb.z); |
521 | } |
522 | |
523 | // ---------------------------------------------------------------------------------------- |
524 | template <typename TReal> |
525 | inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromEulerAnglesXYZ(TReal x, TReal y, TReal z) |
526 | { |
527 | aiMatrix4x4t<TReal>& _this = *this; |
528 | |
529 | TReal cx = std::cos(x); |
530 | TReal sx = std::sin(x); |
531 | TReal cy = std::cos(y); |
532 | TReal sy = std::sin(y); |
533 | TReal cz = std::cos(z); |
534 | TReal sz = std::sin(z); |
535 | |
536 | // mz*my*mx |
537 | _this.a1 = cz * cy; |
538 | _this.a2 = cz * sy * sx - sz * cx; |
539 | _this.a3 = sz * sx + cz * sy * cx; |
540 | |
541 | _this.b1 = sz * cy; |
542 | _this.b2 = cz * cx + sz * sy * sx; |
543 | _this.b3 = sz * sy * cx - cz * sx; |
544 | |
545 | _this.c1 = -sy; |
546 | _this.c2 = cy * sx; |
547 | _this.c3 = cy * cx; |
548 | |
549 | return *this; |
550 | } |
551 | |
552 | // ---------------------------------------------------------------------------------------- |
553 | template <typename TReal> |
554 | inline bool aiMatrix4x4t<TReal>::IsIdentity() const |
555 | { |
556 | // Use a small epsilon to solve floating-point inaccuracies |
557 | const static TReal epsilon = 10e-3f; |
558 | |
559 | return (a2 <= epsilon && a2 >= -epsilon && |
560 | a3 <= epsilon && a3 >= -epsilon && |
561 | a4 <= epsilon && a4 >= -epsilon && |
562 | b1 <= epsilon && b1 >= -epsilon && |
563 | b3 <= epsilon && b3 >= -epsilon && |
564 | b4 <= epsilon && b4 >= -epsilon && |
565 | c1 <= epsilon && c1 >= -epsilon && |
566 | c2 <= epsilon && c2 >= -epsilon && |
567 | c4 <= epsilon && c4 >= -epsilon && |
568 | d1 <= epsilon && d1 >= -epsilon && |
569 | d2 <= epsilon && d2 >= -epsilon && |
570 | d3 <= epsilon && d3 >= -epsilon && |
571 | a1 <= 1.f+epsilon && a1 >= 1.f-epsilon && |
572 | b2 <= 1.f+epsilon && b2 >= 1.f-epsilon && |
573 | c3 <= 1.f+epsilon && c3 >= 1.f-epsilon && |
574 | d4 <= 1.f+epsilon && d4 >= 1.f-epsilon); |
575 | } |
576 | |
577 | // ---------------------------------------------------------------------------------------- |
578 | template <typename TReal> |
579 | inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationX(TReal a, aiMatrix4x4t<TReal>& out) |
580 | { |
581 | /* |
582 | | 1 0 0 0 | |
583 | M = | 0 cos(A) -sin(A) 0 | |
584 | | 0 sin(A) cos(A) 0 | |
585 | | 0 0 0 1 | */ |
586 | out = aiMatrix4x4t<TReal>(); |
587 | out.b2 = out.c3 = std::cos(a); |
588 | out.b3 = -(out.c2 = std::sin(a)); |
589 | return out; |
590 | } |
591 | |
592 | // ---------------------------------------------------------------------------------------- |
593 | template <typename TReal> |
594 | inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationY(TReal a, aiMatrix4x4t<TReal>& out) |
595 | { |
596 | /* |
597 | | cos(A) 0 sin(A) 0 | |
598 | M = | 0 1 0 0 | |
599 | | -sin(A) 0 cos(A) 0 | |
600 | | 0 0 0 1 | |
601 | */ |
602 | out = aiMatrix4x4t<TReal>(); |
603 | out.a1 = out.c3 = std::cos(a); |
604 | out.c1 = -(out.a3 = std::sin(a)); |
605 | return out; |
606 | } |
607 | |
608 | // ---------------------------------------------------------------------------------------- |
609 | template <typename TReal> |
610 | inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::RotationZ(TReal a, aiMatrix4x4t<TReal>& out) |
611 | { |
612 | /* |
613 | | cos(A) -sin(A) 0 0 | |
614 | M = | sin(A) cos(A) 0 0 | |
615 | | 0 0 1 0 | |
616 | | 0 0 0 1 | */ |
617 | out = aiMatrix4x4t<TReal>(); |
618 | out.a1 = out.b2 = std::cos(a); |
619 | out.a2 = -(out.b1 = std::sin(a)); |
620 | return out; |
621 | } |
622 | |
623 | // ---------------------------------------------------------------------------------------- |
624 | // Returns a rotation matrix for a rotation around an arbitrary axis. |
625 | template <typename TReal> |
626 | inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Rotation( TReal a, const aiVector3t<TReal>& axis, aiMatrix4x4t<TReal>& out) |
627 | { |
628 | TReal c = std::cos( a), s = std::sin( a), t = 1 - c; |
629 | TReal x = axis.x, y = axis.y, z = axis.z; |
630 | |
631 | // Many thanks to MathWorld and Wikipedia |
632 | out.a1 = t*x*x + c; out.a2 = t*x*y - s*z; out.a3 = t*x*z + s*y; |
633 | out.b1 = t*x*y + s*z; out.b2 = t*y*y + c; out.b3 = t*y*z - s*x; |
634 | out.c1 = t*x*z - s*y; out.c2 = t*y*z + s*x; out.c3 = t*z*z + c; |
635 | out.a4 = out.b4 = out.c4 = static_cast<TReal>(0.0); |
636 | out.d1 = out.d2 = out.d3 = static_cast<TReal>(0.0); |
637 | out.d4 = static_cast<TReal>(1.0); |
638 | |
639 | return out; |
640 | } |
641 | |
642 | // ---------------------------------------------------------------------------------------- |
643 | template <typename TReal> |
644 | inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Translation( const aiVector3t<TReal>& v, aiMatrix4x4t<TReal>& out) |
645 | { |
646 | out = aiMatrix4x4t<TReal>(); |
647 | out.a4 = v.x; |
648 | out.b4 = v.y; |
649 | out.c4 = v.z; |
650 | return out; |
651 | } |
652 | |
653 | // ---------------------------------------------------------------------------------------- |
654 | template <typename TReal> |
655 | inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::Scaling( const aiVector3t<TReal>& v, aiMatrix4x4t<TReal>& out) |
656 | { |
657 | out = aiMatrix4x4t<TReal>(); |
658 | out.a1 = v.x; |
659 | out.b2 = v.y; |
660 | out.c3 = v.z; |
661 | return out; |
662 | } |
663 | |
664 | // ---------------------------------------------------------------------------------------- |
665 | /** A function for creating a rotation matrix that rotates a vector called |
666 | * "from" into another vector called "to". |
667 | * Input : from[3], to[3] which both must be *normalized* non-zero vectors |
668 | * Output: mtx[3][3] -- a 3x3 matrix in colum-major form |
669 | * Authors: Tomas Möller, John Hughes |
670 | * "Efficiently Building a Matrix to Rotate One Vector to Another" |
671 | * Journal of Graphics Tools, 4(4):1-4, 1999 |
672 | */ |
673 | // ---------------------------------------------------------------------------------------- |
674 | template <typename TReal> |
675 | inline aiMatrix4x4t<TReal>& aiMatrix4x4t<TReal>::FromToMatrix(const aiVector3t<TReal>& from, |
676 | const aiVector3t<TReal>& to, aiMatrix4x4t<TReal>& mtx) |
677 | { |
678 | aiMatrix3x3t<TReal> m3; |
679 | aiMatrix3x3t<TReal>::FromToMatrix(from,to,m3); |
680 | mtx = aiMatrix4x4t<TReal>(m3); |
681 | return mtx; |
682 | } |
683 | |
684 | #endif // __cplusplus |
685 | #endif // AI_MATRIX4X4_INL_INC |
686 | |