miniassimp
view include/miniassimp/matrix3x3.inl @ 0:879c81d94345
initial commit
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Mon, 28 Jan 2019 18:19:26 +0200 |
| parents | |
| children |
line source
1 /*
2 ---------------------------------------------------------------------------
3 Open Asset Import Library (assimp)
4 ---------------------------------------------------------------------------
6 Copyright (c) 2006-2018, assimp team
10 All rights reserved.
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:
16 * Redistributions of source code must retain the above
17 copyright notice, this list of conditions and the
18 following disclaimer.
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.
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.
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 */
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
51 #ifdef __cplusplus
52 #include "matrix3x3.h"
54 #include "matrix4x4.h"
55 #include <algorithm>
56 #include <cmath>
57 #include <limits>
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 }
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 }
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 }
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 }
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 }
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 }
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 }
145 // ------------------------------------------------------------------------------------------------
146 template <typename TReal>
147 inline bool aiMatrix3x3t<TReal>::operator!= (const aiMatrix4x4t<TReal>& m) const
148 {
149 return !(*this == m);
150 }
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 }
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 }
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 }
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);
200 return *this;
201 }
203 TReal invdet = static_cast<TReal>(1.0) / det;
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;
217 return *this;
218 }
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;
228 out.a3 = out.b3 = out.c1 = out.c2 = 0.f;
229 out.c3 = 1.f;
231 return out;
232 }
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;
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;
247 return out;
248 }
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 }
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;
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" */
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;
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 }
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;
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);
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;
344 mtx[1][0] = hvxy + v.z;
345 mtx[1][1] = e + h * v.y * v.y;
346 mtx[1][2] = hvyz - v.x;
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 }
356 #endif // __cplusplus
357 #endif // AI_MATRIX3X3_INL_INC