1 | /* |
2 | --------------------------------------------------------------------------- |
3 | Open Asset Import Library (assimp) |
4 | --------------------------------------------------------------------------- |
5 | |
6 | Copyright (c) 2006-2019, assimp team |
7 | |
8 | |
9 | |
10 | All rights reserved. |
11 | |
12 | Redistribution and use of this software in source and binary forms, |
13 | with or without modification, are permitted provided that the following |
14 | conditions are met: |
15 | |
16 | * Redistributions of source code must retain the above |
17 | copyright notice, this list of conditions and the |
18 | following disclaimer. |
19 | |
20 | * Redistributions in binary form must reproduce the above |
21 | copyright notice, this list of conditions and the |
22 | following disclaimer in the documentation and/or other |
23 | materials provided with the distribution. |
24 | |
25 | * Neither the name of the assimp team, nor the names of its |
26 | contributors may be used to endorse or promote products |
27 | derived from this software without specific prior |
28 | written permission of the assimp team. |
29 | |
30 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
31 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
32 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
33 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |
34 | OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
35 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
36 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |
37 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |
38 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
39 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
40 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
41 | --------------------------------------------------------------------------- |
42 | */ |
43 | |
44 | /** @file matrix3x3.inl |
45 | * @brief Inline implementation of the 3x3 matrix operators |
46 | */ |
47 | #pragma once |
48 | #ifndef AI_MATRIX3X3_INL_INC |
49 | #define AI_MATRIX3X3_INL_INC |
50 | |
51 | #ifdef __cplusplus |
52 | #include "matrix3x3.h" |
53 | |
54 | #include "matrix4x4.h" |
55 | #include <algorithm> |
56 | #include <cmath> |
57 | #include <limits> |
58 | |
59 | // ------------------------------------------------------------------------------------------------ |
60 | // Construction from a 4x4 matrix. The remaining parts of the matrix are ignored. |
61 | template <typename TReal> |
62 | inline aiMatrix3x3t<TReal>::aiMatrix3x3t( const aiMatrix4x4t<TReal>& pMatrix) |
63 | { |
64 | a1 = pMatrix.a1; a2 = pMatrix.a2; a3 = pMatrix.a3; |
65 | b1 = pMatrix.b1; b2 = pMatrix.b2; b3 = pMatrix.b3; |
66 | c1 = pMatrix.c1; c2 = pMatrix.c2; c3 = pMatrix.c3; |
67 | } |
68 | |
69 | // ------------------------------------------------------------------------------------------------ |
70 | template <typename TReal> |
71 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::operator *= (const aiMatrix3x3t<TReal>& m) |
72 | { |
73 | *this = aiMatrix3x3t<TReal>(m.a1 * a1 + m.b1 * a2 + m.c1 * a3, |
74 | m.a2 * a1 + m.b2 * a2 + m.c2 * a3, |
75 | m.a3 * a1 + m.b3 * a2 + m.c3 * a3, |
76 | m.a1 * b1 + m.b1 * b2 + m.c1 * b3, |
77 | m.a2 * b1 + m.b2 * b2 + m.c2 * b3, |
78 | m.a3 * b1 + m.b3 * b2 + m.c3 * b3, |
79 | m.a1 * c1 + m.b1 * c2 + m.c1 * c3, |
80 | m.a2 * c1 + m.b2 * c2 + m.c2 * c3, |
81 | m.a3 * c1 + m.b3 * c2 + m.c3 * c3); |
82 | return *this; |
83 | } |
84 | |
85 | // ------------------------------------------------------------------------------------------------ |
86 | template <typename TReal> |
87 | template <typename TOther> |
88 | aiMatrix3x3t<TReal>::operator aiMatrix3x3t<TOther> () const |
89 | { |
90 | return aiMatrix3x3t<TOther>(static_cast<TOther>(a1),static_cast<TOther>(a2),static_cast<TOther>(a3), |
91 | static_cast<TOther>(b1),static_cast<TOther>(b2),static_cast<TOther>(b3), |
92 | static_cast<TOther>(c1),static_cast<TOther>(c2),static_cast<TOther>(c3)); |
93 | } |
94 | |
95 | // ------------------------------------------------------------------------------------------------ |
96 | template <typename TReal> |
97 | inline aiMatrix3x3t<TReal> aiMatrix3x3t<TReal>::operator* (const aiMatrix3x3t<TReal>& m) const |
98 | { |
99 | aiMatrix3x3t<TReal> temp( *this); |
100 | temp *= m; |
101 | return temp; |
102 | } |
103 | |
104 | // ------------------------------------------------------------------------------------------------ |
105 | template <typename TReal> |
106 | inline TReal* aiMatrix3x3t<TReal>::operator[] (unsigned int p_iIndex) { |
107 | switch ( p_iIndex ) { |
108 | case 0: |
109 | return &a1; |
110 | case 1: |
111 | return &b1; |
112 | case 2: |
113 | return &c1; |
114 | default: |
115 | break; |
116 | } |
117 | return &a1; |
118 | } |
119 | |
120 | // ------------------------------------------------------------------------------------------------ |
121 | template <typename TReal> |
122 | inline const TReal* aiMatrix3x3t<TReal>::operator[] (unsigned int p_iIndex) const { |
123 | switch ( p_iIndex ) { |
124 | case 0: |
125 | return &a1; |
126 | case 1: |
127 | return &b1; |
128 | case 2: |
129 | return &c1; |
130 | default: |
131 | break; |
132 | } |
133 | return &a1; |
134 | } |
135 | |
136 | // ------------------------------------------------------------------------------------------------ |
137 | template <typename TReal> |
138 | inline bool aiMatrix3x3t<TReal>::operator== (const aiMatrix4x4t<TReal>& m) const |
139 | { |
140 | return a1 == m.a1 && a2 == m.a2 && a3 == m.a3 && |
141 | b1 == m.b1 && b2 == m.b2 && b3 == m.b3 && |
142 | c1 == m.c1 && c2 == m.c2 && c3 == m.c3; |
143 | } |
144 | |
145 | // ------------------------------------------------------------------------------------------------ |
146 | template <typename TReal> |
147 | inline bool aiMatrix3x3t<TReal>::operator!= (const aiMatrix4x4t<TReal>& m) const |
148 | { |
149 | return !(*this == m); |
150 | } |
151 | |
152 | // --------------------------------------------------------------------------- |
153 | template<typename TReal> |
154 | inline bool aiMatrix3x3t<TReal>::Equal(const aiMatrix4x4t<TReal>& m, TReal epsilon) const { |
155 | return |
156 | std::abs(a1 - m.a1) <= epsilon && |
157 | std::abs(a2 - m.a2) <= epsilon && |
158 | std::abs(a3 - m.a3) <= epsilon && |
159 | std::abs(b1 - m.b1) <= epsilon && |
160 | std::abs(b2 - m.b2) <= epsilon && |
161 | std::abs(b3 - m.b3) <= epsilon && |
162 | std::abs(c1 - m.c1) <= epsilon && |
163 | std::abs(c2 - m.c2) <= epsilon && |
164 | std::abs(c3 - m.c3) <= epsilon; |
165 | } |
166 | |
167 | // ------------------------------------------------------------------------------------------------ |
168 | template <typename TReal> |
169 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Transpose() |
170 | { |
171 | // (TReal&) don't remove, GCC complains cause of packed fields |
172 | std::swap( (TReal&)a2, (TReal&)b1); |
173 | std::swap( (TReal&)a3, (TReal&)c1); |
174 | std::swap( (TReal&)b3, (TReal&)c2); |
175 | return *this; |
176 | } |
177 | |
178 | // ---------------------------------------------------------------------------------------- |
179 | template <typename TReal> |
180 | inline TReal aiMatrix3x3t<TReal>::Determinant() const |
181 | { |
182 | return a1*b2*c3 - a1*b3*c2 + a2*b3*c1 - a2*b1*c3 + a3*b1*c2 - a3*b2*c1; |
183 | } |
184 | |
185 | // ---------------------------------------------------------------------------------------- |
186 | template <typename TReal> |
187 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Inverse() |
188 | { |
189 | // Compute the reciprocal determinant |
190 | TReal det = Determinant(); |
191 | if(det == static_cast<TReal>(0.0)) |
192 | { |
193 | // Matrix not invertible. Setting all elements to nan is not really |
194 | // correct in a mathematical sense; but at least qnans are easy to |
195 | // spot. XXX we might throw an exception instead, which would |
196 | // be even much better to spot :/. |
197 | const TReal nan = std::numeric_limits<TReal>::quiet_NaN(); |
198 | *this = aiMatrix3x3t<TReal>( nan,nan,nan,nan,nan,nan,nan,nan,nan); |
199 | |
200 | return *this; |
201 | } |
202 | |
203 | TReal invdet = static_cast<TReal>(1.0) / det; |
204 | |
205 | aiMatrix3x3t<TReal> res; |
206 | res.a1 = invdet * (b2 * c3 - b3 * c2); |
207 | res.a2 = -invdet * (a2 * c3 - a3 * c2); |
208 | res.a3 = invdet * (a2 * b3 - a3 * b2); |
209 | res.b1 = -invdet * (b1 * c3 - b3 * c1); |
210 | res.b2 = invdet * (a1 * c3 - a3 * c1); |
211 | res.b3 = -invdet * (a1 * b3 - a3 * b1); |
212 | res.c1 = invdet * (b1 * c2 - b2 * c1); |
213 | res.c2 = -invdet * (a1 * c2 - a2 * c1); |
214 | res.c3 = invdet * (a1 * b2 - a2 * b1); |
215 | *this = res; |
216 | |
217 | return *this; |
218 | } |
219 | |
220 | // ------------------------------------------------------------------------------------------------ |
221 | template <typename TReal> |
222 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::RotationZ(TReal a, aiMatrix3x3t<TReal>& out) |
223 | { |
224 | out.a1 = out.b2 = std::cos(a); |
225 | out.b1 = std::sin(a); |
226 | out.a2 = - out.b1; |
227 | |
228 | out.a3 = out.b3 = out.c1 = out.c2 = 0.f; |
229 | out.c3 = 1.f; |
230 | |
231 | return out; |
232 | } |
233 | |
234 | // ------------------------------------------------------------------------------------------------ |
235 | // Returns a rotation matrix for a rotation around an arbitrary axis. |
236 | template <typename TReal> |
237 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Rotation( TReal a, const aiVector3t<TReal>& axis, aiMatrix3x3t<TReal>& out) |
238 | { |
239 | TReal c = std::cos( a), s = std::sin( a), t = 1 - c; |
240 | TReal x = axis.x, y = axis.y, z = axis.z; |
241 | |
242 | // Many thanks to MathWorld and Wikipedia |
243 | out.a1 = t*x*x + c; out.a2 = t*x*y - s*z; out.a3 = t*x*z + s*y; |
244 | out.b1 = t*x*y + s*z; out.b2 = t*y*y + c; out.b3 = t*y*z - s*x; |
245 | out.c1 = t*x*z - s*y; out.c2 = t*y*z + s*x; out.c3 = t*z*z + c; |
246 | |
247 | return out; |
248 | } |
249 | |
250 | // ------------------------------------------------------------------------------------------------ |
251 | template <typename TReal> |
252 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Translation( const aiVector2t<TReal>& v, aiMatrix3x3t<TReal>& out) |
253 | { |
254 | out = aiMatrix3x3t<TReal>(); |
255 | out.a3 = v.x; |
256 | out.b3 = v.y; |
257 | return out; |
258 | } |
259 | |
260 | // ---------------------------------------------------------------------------------------- |
261 | /** A function for creating a rotation matrix that rotates a vector called |
262 | * "from" into another vector called "to". |
263 | * Input : from[3], to[3] which both must be *normalized* non-zero vectors |
264 | * Output: mtx[3][3] -- a 3x3 matrix in colum-major form |
265 | * Authors: Tomas Möller, John Hughes |
266 | * "Efficiently Building a Matrix to Rotate One Vector to Another" |
267 | * Journal of Graphics Tools, 4(4):1-4, 1999 |
268 | */ |
269 | // ---------------------------------------------------------------------------------------- |
270 | template <typename TReal> |
271 | inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::FromToMatrix(const aiVector3t<TReal>& from, |
272 | const aiVector3t<TReal>& to, aiMatrix3x3t<TReal>& mtx) |
273 | { |
274 | const TReal e = from * to; |
275 | const TReal f = (e < 0)? -e:e; |
276 | |
277 | if (f > static_cast<TReal>(1.0) - static_cast<TReal>(0.00001)) /* "from" and "to"-vector almost parallel */ |
278 | { |
279 | aiVector3D u,v; /* temporary storage vectors */ |
280 | aiVector3D x; /* vector most nearly orthogonal to "from" */ |
281 | |
282 | x.x = (from.x > 0.0)? from.x : -from.x; |
283 | x.y = (from.y > 0.0)? from.y : -from.y; |
284 | x.z = (from.z > 0.0)? from.z : -from.z; |
285 | |
286 | if (x.x < x.y) |
287 | { |
288 | if (x.x < x.z) |
289 | { |
290 | x.x = static_cast<TReal>(1.0); |
291 | x.y = x.z = static_cast<TReal>(0.0); |
292 | } |
293 | else |
294 | { |
295 | x.z = static_cast<TReal>(1.0); |
296 | x.x = x.y = static_cast<TReal>(0.0); |
297 | } |
298 | } |
299 | else |
300 | { |
301 | if (x.y < x.z) |
302 | { |
303 | x.y = static_cast<TReal>(1.0); |
304 | x.x = x.z = static_cast<TReal>(0.0); |
305 | } |
306 | else |
307 | { |
308 | x.z = static_cast<TReal>(1.0); |
309 | x.x = x.y = static_cast<TReal>(0.0); |
310 | } |
311 | } |
312 | |
313 | u.x = x.x - from.x; u.y = x.y - from.y; u.z = x.z - from.z; |
314 | v.x = x.x - to.x; v.y = x.y - to.y; v.z = x.z - to.z; |
315 | |
316 | const TReal c1_ = static_cast<TReal>(2.0) / (u * u); |
317 | const TReal c2_ = static_cast<TReal>(2.0) / (v * v); |
318 | const TReal c3_ = c1_ * c2_ * (u * v); |
319 | |
320 | for (unsigned int i = 0; i < 3; i++) |
321 | { |
322 | for (unsigned int j = 0; j < 3; j++) |
323 | { |
324 | mtx[i][j] = - c1_ * u[i] * u[j] - c2_ * v[i] * v[j] |
325 | + c3_ * v[i] * u[j]; |
326 | } |
327 | mtx[i][i] += static_cast<TReal>(1.0); |
328 | } |
329 | } |
330 | else /* the most common case, unless "from"="to", or "from"=-"to" */ |
331 | { |
332 | const aiVector3D v = from ^ to; |
333 | /* ... use this hand optimized version (9 mults less) */ |
334 | const TReal h = static_cast<TReal>(1.0)/(static_cast<TReal>(1.0) + e); /* optimization by Gottfried Chen */ |
335 | const TReal hvx = h * v.x; |
336 | const TReal hvz = h * v.z; |
337 | const TReal hvxy = hvx * v.y; |
338 | const TReal hvxz = hvx * v.z; |
339 | const TReal hvyz = hvz * v.y; |
340 | mtx[0][0] = e + hvx * v.x; |
341 | mtx[0][1] = hvxy - v.z; |
342 | mtx[0][2] = hvxz + v.y; |
343 | |
344 | mtx[1][0] = hvxy + v.z; |
345 | mtx[1][1] = e + h * v.y * v.y; |
346 | mtx[1][2] = hvyz - v.x; |
347 | |
348 | mtx[2][0] = hvxz - v.y; |
349 | mtx[2][1] = hvyz + v.x; |
350 | mtx[2][2] = e + hvz * v.z; |
351 | } |
352 | return mtx; |
353 | } |
354 | |
355 | |
356 | #endif // __cplusplus |
357 | #endif // AI_MATRIX3X3_INL_INC |
358 | |