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 matrix3x3.inl
44 * @brief Inline implementation of the 3x3 matrix operators
45 */
46#pragma once
47#ifndef AI_MATRIX3X3_INL_INC
48#define AI_MATRIX3X3_INL_INC
49
50#ifdef __cplusplus
51#include "matrix3x3.h"
52
53#include "matrix4x4.h"
54#include <algorithm>
55#include <cmath>
56#include <limits>
57
58// ------------------------------------------------------------------------------------------------
59// Construction from a 4x4 matrix. The remaining parts of the matrix are ignored.
60template <typename TReal>
61inline aiMatrix3x3t<TReal>::aiMatrix3x3t( const aiMatrix4x4t<TReal>& pMatrix)
62{
63 a1 = pMatrix.a1; a2 = pMatrix.a2; a3 = pMatrix.a3;
64 b1 = pMatrix.b1; b2 = pMatrix.b2; b3 = pMatrix.b3;
65 c1 = pMatrix.c1; c2 = pMatrix.c2; c3 = pMatrix.c3;
66}
67
68// ------------------------------------------------------------------------------------------------
69template <typename TReal>
70inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::operator *= (const aiMatrix3x3t<TReal>& m)
71{
72 *this = aiMatrix3x3t<TReal>(m.a1 * a1 + m.b1 * a2 + m.c1 * a3,
73 m.a2 * a1 + m.b2 * a2 + m.c2 * a3,
74 m.a3 * a1 + m.b3 * a2 + m.c3 * a3,
75 m.a1 * b1 + m.b1 * b2 + m.c1 * b3,
76 m.a2 * b1 + m.b2 * b2 + m.c2 * b3,
77 m.a3 * b1 + m.b3 * b2 + m.c3 * b3,
78 m.a1 * c1 + m.b1 * c2 + m.c1 * c3,
79 m.a2 * c1 + m.b2 * c2 + m.c2 * c3,
80 m.a3 * c1 + m.b3 * c2 + m.c3 * c3);
81 return *this;
82}
83
84// ------------------------------------------------------------------------------------------------
85template <typename TReal>
86template <typename TOther>
87aiMatrix3x3t<TReal>::operator aiMatrix3x3t<TOther> () const
88{
89 return aiMatrix3x3t<TOther>(static_cast<TOther>(a1),static_cast<TOther>(a2),static_cast<TOther>(a3),
90 static_cast<TOther>(b1),static_cast<TOther>(b2),static_cast<TOther>(b3),
91 static_cast<TOther>(c1),static_cast<TOther>(c2),static_cast<TOther>(c3));
92}
93
94// ------------------------------------------------------------------------------------------------
95template <typename TReal>
96inline aiMatrix3x3t<TReal> aiMatrix3x3t<TReal>::operator* (const aiMatrix3x3t<TReal>& m) const
97{
98 aiMatrix3x3t<TReal> temp( *this);
99 temp *= m;
100 return temp;
101}
102
103// ------------------------------------------------------------------------------------------------
104template <typename TReal>
105inline TReal* aiMatrix3x3t<TReal>::operator[] (unsigned int p_iIndex) {
106 switch ( p_iIndex ) {
107 case 0:
108 return &a1;
109 case 1:
110 return &b1;
111 case 2:
112 return &c1;
113 default:
114 break;
115 }
116 return &a1;
117}
118
119// ------------------------------------------------------------------------------------------------
120template <typename TReal>
121inline const TReal* aiMatrix3x3t<TReal>::operator[] (unsigned int p_iIndex) const {
122 switch ( p_iIndex ) {
123 case 0:
124 return &a1;
125 case 1:
126 return &b1;
127 case 2:
128 return &c1;
129 default:
130 break;
131 }
132 return &a1;
133}
134
135// ------------------------------------------------------------------------------------------------
136template <typename TReal>
137inline bool aiMatrix3x3t<TReal>::operator== (const aiMatrix4x4t<TReal>& m) const
138{
139 return a1 == m.a1 && a2 == m.a2 && a3 == m.a3 &&
140 b1 == m.b1 && b2 == m.b2 && b3 == m.b3 &&
141 c1 == m.c1 && c2 == m.c2 && c3 == m.c3;
142}
143
144// ------------------------------------------------------------------------------------------------
145template <typename TReal>
146inline bool aiMatrix3x3t<TReal>::operator!= (const aiMatrix4x4t<TReal>& m) const
147{
148 return !(*this == m);
149}
150
151// ---------------------------------------------------------------------------
152template<typename TReal>
153inline bool aiMatrix3x3t<TReal>::Equal(const aiMatrix4x4t<TReal>& m, TReal epsilon) const {
154 return
155 std::abs(a1 - m.a1) <= epsilon &&
156 std::abs(a2 - m.a2) <= epsilon &&
157 std::abs(a3 - m.a3) <= epsilon &&
158 std::abs(b1 - m.b1) <= epsilon &&
159 std::abs(b2 - m.b2) <= epsilon &&
160 std::abs(b3 - m.b3) <= epsilon &&
161 std::abs(c1 - m.c1) <= epsilon &&
162 std::abs(c2 - m.c2) <= epsilon &&
163 std::abs(c3 - m.c3) <= epsilon;
164}
165
166// ------------------------------------------------------------------------------------------------
167template <typename TReal>
168inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Transpose()
169{
170 // (TReal&) don't remove, GCC complains cause of packed fields
171 std::swap( (TReal&)a2, (TReal&)b1);
172 std::swap( (TReal&)a3, (TReal&)c1);
173 std::swap( (TReal&)b3, (TReal&)c2);
174 return *this;
175}
176
177// ----------------------------------------------------------------------------------------
178template <typename TReal>
179inline TReal aiMatrix3x3t<TReal>::Determinant() const
180{
181 return a1*b2*c3 - a1*b3*c2 + a2*b3*c1 - a2*b1*c3 + a3*b1*c2 - a3*b2*c1;
182}
183
184// ----------------------------------------------------------------------------------------
185template <typename TReal>
186inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Inverse()
187{
188 // Compute the reciprocal determinant
189 TReal det = Determinant();
190 if(det == static_cast<TReal>(0.0))
191 {
192 // Matrix not invertible. Setting all elements to nan is not really
193 // correct in a mathematical sense; but at least qnans are easy to
194 // spot. XXX we might throw an exception instead, which would
195 // be even much better to spot :/.
196 const TReal nan = std::numeric_limits<TReal>::quiet_NaN();
197 *this = aiMatrix3x3t<TReal>( nan,nan,nan,nan,nan,nan,nan,nan,nan);
198
199 return *this;
200 }
201
202 TReal invdet = static_cast<TReal>(1.0) / det;
203
204 aiMatrix3x3t<TReal> res;
205 res.a1 = invdet * (b2 * c3 - b3 * c2);
206 res.a2 = -invdet * (a2 * c3 - a3 * c2);
207 res.a3 = invdet * (a2 * b3 - a3 * b2);
208 res.b1 = -invdet * (b1 * c3 - b3 * c1);
209 res.b2 = invdet * (a1 * c3 - a3 * c1);
210 res.b3 = -invdet * (a1 * b3 - a3 * b1);
211 res.c1 = invdet * (b1 * c2 - b2 * c1);
212 res.c2 = -invdet * (a1 * c2 - a2 * c1);
213 res.c3 = invdet * (a1 * b2 - a2 * b1);
214 *this = res;
215
216 return *this;
217}
218
219// ------------------------------------------------------------------------------------------------
220template <typename TReal>
221inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::RotationZ(TReal a, aiMatrix3x3t<TReal>& out)
222{
223 out.a1 = out.b2 = std::cos(a);
224 out.b1 = std::sin(a);
225 out.a2 = - out.b1;
226
227 out.a3 = out.b3 = out.c1 = out.c2 = 0.f;
228 out.c3 = 1.f;
229
230 return out;
231}
232
233// ------------------------------------------------------------------------------------------------
234// Returns a rotation matrix for a rotation around an arbitrary axis.
235template <typename TReal>
236inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Rotation( TReal a, const aiVector3t<TReal>& axis, aiMatrix3x3t<TReal>& out)
237{
238 TReal c = std::cos( a), s = std::sin( a), t = 1 - c;
239 TReal x = axis.x, y = axis.y, z = axis.z;
240
241 // Many thanks to MathWorld and Wikipedia
242 out.a1 = t*x*x + c; out.a2 = t*x*y - s*z; out.a3 = t*x*z + s*y;
243 out.b1 = t*x*y + s*z; out.b2 = t*y*y + c; out.b3 = t*y*z - s*x;
244 out.c1 = t*x*z - s*y; out.c2 = t*y*z + s*x; out.c3 = t*z*z + c;
245
246 return out;
247}
248
249// ------------------------------------------------------------------------------------------------
250template <typename TReal>
251inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::Translation( const aiVector2t<TReal>& v, aiMatrix3x3t<TReal>& out)
252{
253 out = aiMatrix3x3t<TReal>();
254 out.a3 = v.x;
255 out.b3 = v.y;
256 return out;
257}
258
259// ----------------------------------------------------------------------------------------
260/** A function for creating a rotation matrix that rotates a vector called
261 * "from" into another vector called "to".
262 * Input : from[3], to[3] which both must be *normalized* non-zero vectors
263 * Output: mtx[3][3] -- a 3x3 matrix in colum-major form
264 * Authors: Tomas Möller, John Hughes
265 * "Efficiently Building a Matrix to Rotate One Vector to Another"
266 * Journal of Graphics Tools, 4(4):1-4, 1999
267 */
268// ----------------------------------------------------------------------------------------
269template <typename TReal>
270inline aiMatrix3x3t<TReal>& aiMatrix3x3t<TReal>::FromToMatrix(const aiVector3t<TReal>& from,
271 const aiVector3t<TReal>& to, aiMatrix3x3t<TReal>& mtx)
272{
273 const TReal e = from * to;
274 const TReal f = (e < 0)? -e:e;
275
276 if (f > static_cast<TReal>(1.0) - static_cast<TReal>(0.00001)) /* "from" and "to"-vector almost parallel */
277 {
278 aiVector3D u,v; /* temporary storage vectors */
279 aiVector3D x; /* vector most nearly orthogonal to "from" */
280
281 x.x = (from.x > 0.0)? from.x : -from.x;
282 x.y = (from.y > 0.0)? from.y : -from.y;
283 x.z = (from.z > 0.0)? from.z : -from.z;
284
285 if (x.x < x.y)
286 {
287 if (x.x < x.z)
288 {
289 x.x = static_cast<TReal>(1.0);
290 x.y = x.z = static_cast<TReal>(0.0);
291 }
292 else
293 {
294 x.z = static_cast<TReal>(1.0);
295 x.x = x.y = static_cast<TReal>(0.0);
296 }
297 }
298 else
299 {
300 if (x.y < x.z)
301 {
302 x.y = static_cast<TReal>(1.0);
303 x.x = x.z = static_cast<TReal>(0.0);
304 }
305 else
306 {
307 x.z = static_cast<TReal>(1.0);
308 x.x = x.y = static_cast<TReal>(0.0);
309 }
310 }
311
312 u.x = x.x - from.x; u.y = x.y - from.y; u.z = x.z - from.z;
313 v.x = x.x - to.x; v.y = x.y - to.y; v.z = x.z - to.z;
314
315 const TReal c1 = static_cast<TReal>(2.0) / (u * u);
316 const TReal c2 = static_cast<TReal>(2.0) / (v * v);
317 const TReal c3 = c1 * c2 * (u * v);
318
319 for (unsigned int i = 0; i < 3; i++)
320 {
321 for (unsigned int j = 0; j < 3; j++)
322 {
323 mtx[i][j] = - c1 * u[i] * u[j] - c2 * v[i] * v[j]
324 + c3 * v[i] * u[j];
325 }
326 mtx[i][i] += static_cast<TReal>(1.0);
327 }
328 }
329 else /* the most common case, unless "from"="to", or "from"=-"to" */
330 {
331 const aiVector3D v = from ^ to;
332 /* ... use this hand optimized version (9 mults less) */
333 const TReal h = static_cast<TReal>(1.0)/(static_cast<TReal>(1.0) + e); /* optimization by Gottfried Chen */
334 const TReal hvx = h * v.x;
335 const TReal hvz = h * v.z;
336 const TReal hvxy = hvx * v.y;
337 const TReal hvxz = hvx * v.z;
338 const TReal hvyz = hvz * v.y;
339 mtx[0][0] = e + hvx * v.x;
340 mtx[0][1] = hvxy - v.z;
341 mtx[0][2] = hvxz + v.y;
342
343 mtx[1][0] = hvxy + v.z;
344 mtx[1][1] = e + h * v.y * v.y;
345 mtx[1][2] = hvyz - v.x;
346
347 mtx[2][0] = hvxz - v.y;
348 mtx[2][1] = hvyz + v.x;
349 mtx[2][2] = e + hvz * v.z;
350 }
351 return mtx;
352}
353
354
355#endif // __cplusplus
356#endif // AI_MATRIX3X3_INL_INC
357