Matrix.inl

1 /* Copyright 2011 Jukka Jyl�nki
2
3    Licensed under the Apache License, Version 2.0 (the "License");
4    you may not use this file except in compliance with the License.
5    You may obtain a copy of the License at
6
7        http://www.apache.org/licenses/LICENSE-2.0
8
9    Unless required by applicable law or agreed to in writing, software
10    distributed under the License is distributed on an "AS IS" BASIS,
11    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12    See the License for the specific language governing permissions and
13    limitations under the License. */
14
15 /** @file Matrix.inl
16         @author Jukka Jyl�nki
17         @brief */
18 #pragma once
19
20 #include <utility>
21 #include "Quat.h"
22
23 MATH_BEGIN_NAMESPACE
24
25 /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the X-axis. Elements
26         outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
27         in the order M*v, and clockwise if rotated in the order v*M.
28         @param m The matrix to store the result.
29         @param angle The rotation angle in radians. */
30 template<typename Matrix>
31 void Set3x3PartRotateX(Matrix &m, float angle)
32 {
33         float sinz, cosz;
34         SinCos(angle, sinz, cosz);
35
36         m[0][0] = 1.f; m[0][1] =  0.f; m[0][2] =   0.f;
37         m[1][0] = 0.f; m[1][1] = cosz; m[1][2] = -sinz;
38         m[2][0] = 0.f; m[2][1] = sinz; m[2][2] =  cosz;
39 }
40
41 /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Y-axis. Elements
42         outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
43         in the order M*v, and clockwise if rotated in the order v*M.
44         @param m The matrix to store the result.
45         @param angle The rotation angle in radians. */
46 template<typename Matrix>
47 void Set3x3PartRotateY(Matrix &m, float angle)
48 {
49         float sinz, cosz;
50         SinCos(angle, sinz, cosz);
51
52         m[0][0] =  cosz; m[0][1] = 0.f; m[0][2] = sinz;
53         m[1][0] =   0.f; m[1][1] = 1.f; m[1][2] =  0.f;
54         m[2][0] = -sinz; m[2][1] = 0.f; m[2][2] = cosz;
55 }
56
57 /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Z-axis. Elements
58         outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
59         in the order M*v, and clockwise if rotated in the order v*M.
60         @param m The matrix to store the result.
61         @param angle The rotation angle in radians. */
62 template<typename Matrix>
63 void Set3x3PartRotateZ(Matrix &m, float angle)
64 {
65         float sinz, cosz;
66         SinCos(angle, sinz, cosz);
67
68         m[0][0] = cosz; m[0][1] = -sinz; m[0][2] = 0.f;
69         m[1][0] = sinz; m[1][1] =  cosz; m[1][2] = 0.f;
70         m[2][0] =  0.f; m[2][1] =   0.f; m[2][2] = 1.f;
71 }
72
73 /** Computes the matrix M = R_x * R_y * R_z, where R_d is the cardinal rotation matrix
74         about the axis +d, rotating counterclockwise.
75         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
76         Parameters x, y and z are the angles of rotation, in radians. */
77 template<typename Matrix>
78 void Set3x3PartRotateEulerXYZ(Matrix &m, float x, float y, float z)
79 {
80         ///\todo Vectorize to compute 4 sines+cosines at one time.
81         float cx, sx, cy, sy, cz, sz;
82         SinCos(x, sx, cx);
83         SinCos(y, sy, cy);
84         SinCos(z, sz, cz);
85
86         m[0][0] =             cy*cz; m[0][1] =         -cy * sz; m[0][2] =     sy;
87         m[1][0] =  cz*sx*sy + cx*sz; m[1][1] = cx*cz - sx*sy*sz; m[1][2] = -cy*sx;
88         m[2][0] = -cx*cz*sy + sx*sz; m[2][1] = cz*sx + cx*sy*sz; m[2][2] =  cx*cy;
89 }
90
91 /** Decomposes the matrix M to form M = R_x * R_y * R_z (R_d being the cardinal rotation
92         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
93         parameters x, y and z, in radians.
94         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
95         The matrix m must be orthonormal. */
96 template<typename Matrix>
97 void ExtractEulerXYZ(Matrix &m, float &x, float &y, float &z)
98 {
99         if (m[0][2] < 1.f)
100         {
101                 if (m[0][2] > -1.f)
102                 {
103                         y = asin(m[0][2]);
104                         x = atan2(-m[1][2], m[2][2]);
105                         z = atan2(-m[0][1], m[0][0]);
106                 }
107                 else
108                 {
109                         // Not a unique solution: z - x = atan2(m[1][0], m[1][1]);
110                         y = -pi/2.f;
111                         x = -atan2(m[1][0], m[1][1]);
112                         z = 0.f;
113                 }
114         }
115         else
116         {
117                 // Not a unique solution: z + x = atan2(m[1][0], m[1][1]);
118                 y = pi/2.f;
119                 x = atan2(m[1][0], m[1][1]);
120                 z = 0.f;
121         }
122 }
123
124 /** Computes the matrix M = R_x * R_z * R_y, where R_d is the cardinal rotation matrix
125         about the axis +d, rotating counterclockwise.
126         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
127         Parameters x, y and z are the angles of rotation, in radians. */
128 template<typename Matrix>
129 void Set3x3PartRotateEulerXZY(Matrix &m, float &x, float &z, float &y)
130 {
131         ///\todo Vectorize to compute 4 sines+cosines at one time.
132         float cx, sx, cy, sy, cz, sz;
133         SinCos(x, sx, cx);
134         SinCos(y, sy, cy);
135         SinCos(z, sz, cz);
136
137         m[0][0] =             cy*cz; m[0][1] =   -sz; m[0][2] =             cz*sy;
138         m[1][0] =  sx*sy + cx*cy*sz; m[1][1] = cx*cz; m[1][2] = -cy*sx + cx*sy*sz;
139         m[2][0] = -cx*sy + cy*sx*sz; m[2][1] = cz*sx; m[2][2] =  cx*cy + sx*sy*sz;
140 }
141
142 /** Decomposes the matrix M to form M = R_x * R_z * R_y (R_d being the cardinal rotation
143         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
144         parameters x, y and z, in radians.
145         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
146         The matrix m must be orthonormal. */
147 template<typename Matrix>
148 void ExtractEulerXZY(Matrix &m, float &x, float &z, float &y)
149 {
150         if (m[0][1] < 1.f)
151         {
152                 if (m[0][1] > -1.f)
153                 {
154                         z = asin(-m[0][1]);
155                         x = atan2(m[2][1], m[1][1]);
156                         y = atan2(m[0][2], m[0][0]);
157                 }
158                 else
159                 {
160                         // Not a unique solution: y - x = atan2(-m[2][0], m[2][2]);
161                         z = pi/2.f;
162                         x = atan2(-m[2][0], m[2][2]);
163                         y = 0;
164                 }
165         }
166         else
167         {
168                 // Not a unique solution: y + x = atan2(-m[2][0], m[2][2]);
169                 z = -pi/2.f;
170                 x = atan2(-m[2][0], m[2][2]);
171                 y = 0;
172         }
173 }
174
175 /** Computes the matrix M = R_y * R_x * R_z, where R_d is the cardinal rotation matrix
176         about the axis +d, rotating counterclockwise.
177         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
178         Parameters x, y and z are the angles of rotation, in radians. */
179 template<typename Matrix>
180 void Set3x3PartRotateEulerYXZ(Matrix &m, float &y, float &x, float &z)
181 {
182         ///\todo Vectorize to compute 4 sines+cosines at one time.
183         float cx, sx, cy, sy, cz, sz;
184         SinCos(x, sx, cx);
185         SinCos(y, sy, cy);
186         SinCos(z, sz, cz);
187
188         m[0][0] =  cy*cz + sx*sy*sz; m[0][1] = cx*sx*sy - cy*sz; m[0][2] = cx*sy;
189         m[1][0] =             cx*sz; m[1][1] = cx*cz;            m[1][2] =   -sx;
190         m[2][0] = -cz*sy + cy*sx*sz; m[2][1] = cy*cz*sx + sy*sz; m[2][2] = cx*cy;
191 }
192
193 /** Decomposes the matrix M to form M = R_y * R_x * R_z (R_d being the cardinal rotation
194         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
195         parameters x, y and z, in radians.
196         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
197         The matrix m must be orthonormal. */
198 template<typename Matrix>
199 void ExtractEulerYXZ(Matrix &m, float &y, float &x, float &z)
200 {
201         if (m[1][2] < 1.f)
202         {
203                 if (m[1][2] > -1.f)
204                 {
205                         x = asin(-m[1][2]);
206                         y = atan2(m[0][2], m[2][2]);
207                         z = atan2(m[1][0], m[1][1]);
208                 }
209                 else
210                 {
211                         // Not a unique solution.
212                         x = pi/2.f;
213                         y = -atan2(-m[0][1], m[0][0]);
214                         z = 0;
215                 }
216         }
217         else
218         {
219                 // Not a unique solution.
220                 x = -pi/2.f;
221                 y = atan2(-m[0][1], m[0][0]);
222                 z = 0;
223         }
224 }
225
226 /** Computes the matrix M = R_y * R_z * R_x, where R_d is the cardinal rotation matrix
227         about the axis +d, rotating counterclockwise.
228         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
229         Parameters x, y and z are the angles of rotation, in radians. */
230 template<typename Matrix>
231 void Set3x3PartRotateEulerYZX(Matrix &m, float &y, float &z, float &x)
232 {
233         ///\todo Vectorize to compute 4 sines+cosines at one time.
234         float cx, sx, cy, sy, cz, sz;
235         SinCos(x, sx, cx);
236         SinCos(y, sy, cy);
237         SinCos(z, sz, cz);
238
239         m[0][0] =  cy*cz; m[0][1] = sx*sy - cx*cy*sz; m[0][2] = cx*sy + cy*sx*sz;
240         m[1][0] =     sz; m[1][1] =            cx*cz; m[1][2] =           -cz*sx;
241         m[2][0] = -cx*sy; m[2][1] = cy*sx + cx*sy*sz; m[2][2] = cx*cy - sx*sy*sz;
242 }
243
244 /** Decomposes the matrix M to form M = R_y * R_z * R_x (R_d being the cardinal rotation
245         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
246         parameters x, y and z, in radians.
247         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
248         The matrix m must be orthonormal. */
249 template<typename Matrix>
250 void ExtractEulerYZX(Matrix &m, float &y, float &z, float &x)
251 {
252         if (m[1][0] < 1.f)
253         {
254                 if (m[1][0] > -1.f)
255                 {
256                         z = asin(m[1][0]);
257                         y = atan2(m[2][0], m[0][0]);
258                         x = atan2(m[1][2], m[1][1]);
259                 }
260                 else
261                 {
262                         // Not a unique solution.
263                         z = -pi/2.f;
264                         y = -atan2(m[2][1], m[2][2]);
265                         x = 0;
266                 }
267         }
268         else
269         {
270                 // Not a unique solution.
271                 z = pi/2.f;
272                 y = atan2(-m[2][1], m[2][2]);
273                 x = 0;
274         }
275 }
276
277 /** Computes the matrix M = R_z * R_x * R_y, where R_d is the cardinal rotation matrix
278         about the axis +d, rotating counterclockwise.
279         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
280         Parameters x, y and z are the angles of rotation, in radians. */
281 template<typename Matrix>
282 void Set3x3PartRotateEulerZXY(Matrix &m, float &z, float &x, float &y)
283 {
284         ///\todo Vectorize to compute 4 sines+cosines at one time.
285         float cx, sx, cy, sy, cz, sz;
286         SinCos(x, sx, cx);
287         SinCos(y, sy, cy);
288         SinCos(z, sz, cz);
289
290         m[0][0] = cy*cz - sx*sy*sz; m[0][1] = -cx*sz; m[0][2] =  cz*sy + cy*sx*sz;
291         m[1][0] = cz*sx*sy + cy*sz; m[1][1] =  cx*cz; m[1][2] = -cy*cz*sx + sy*sz;
292         m[2][0] =           -cx*sy; m[2][1] =     sx; m[2][2] =             cx*cy;
293 }
294
295 /** Decomposes the matrix M to form M = R_z * R_x * R_y (R_d being the cardinal rotation
296         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
297         parameters x, y and z, in radians.
298         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
299         The matrix m must be orthonormal. */
300 template<typename Matrix>
301 void ExtractEulerZXY(const Matrix &m, float &z, float &x, float &y)
302 {
303         if (m[2][1] < 1.f)
304         {
305                 if (m[2][1] > -1.f)
306                 {
307                         x = asin(m[2][1]);
308                         z = atan2(-m[0][1], m[1][1]);
309                         y = atan2(-m[2][0], m[2][2]);
310                 }
311                 else
312                 {
313                         // Not a unique solution.
314                         x = -pi/2.f;
315                         z = -atan2(-m[0][2], m[0][0]);
316                         y = 0;
317                 }
318         }
319         else
320         {
321                 // Not a unique solution.
322                 x = pi/2.f;
323                 z = atan2(m[0][2], m[0][0]);
324                 y = 0;
325         }
326 }
327
328 /** Computes the matrix M = R_z * R_y * R_x, where R_d is the cardinal rotation matrix
329         about the axis +d, rotating counterclockwise.
330         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
331         Parameters x, y and z are the angles of rotation, in radians. */
332 template<typename Matrix>
333 void Set3x3PartRotateEulerZYX(Matrix &m, float &z, float &y, float &x)
334 {
335         ///\todo Vectorize to compute 4 sines+cosines at one time.
336         float cx, sx, cy, sy, cz, sz;
337         SinCos(x, sx, cx);
338         SinCos(y, sy, cy);
339         SinCos(z, sz, cz);
340
341         m[0][0] = cy*cz; m[0][1] = cz*sx*sy - cx*sz; m[0][2] =  cx*cz*sy + sx*sz;
342         m[1][0] = cy*sz; m[1][1] = cx*cz + sx*sy*sz; m[1][2] = -cz*sx + cx*sy*sz;
343         m[2][0] =   -sy; m[2][1] =            cy*sx; m[2][2] =             cx*cy;
344 }
345
346 /** Decomposes the matrix M to form M = R_z * R_y * R_x (R_d being the cardinal rotation
347         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
348         parameters x, y and z, in radians.
349         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
350         The matrix m must be orthonormal. */
351 template<typename Matrix>
352 void ExtractEulerZYX(Matrix &m, float &z, float &y, float &x)
353 {
354         if (m[2][0] < 1.f)
355         {
356                 if (m[2][0] > -1.f)
357                 {
358                         y = asin(-m[2][0]);
359                         z = atan2(m[1][0], m[0][0]);
360                         x = atan2(m[2][1], m[2][2]);
361                 }
362                 else
363                 {
364                         // Not a unique solution.
365                         y = pi/2.f;
366                         z = -atan2(-m[1][2], m[1][1]);
367                         x = 0;
368                 }
369         }
370         else
371         {
372                 // Not a unique solution.
373                 y = -pi/2.f;
374                 z = atan2(-m[1][2], m[1][1]);
375                 x = 0;
376         }
377 }
378
379 /** Computes the matrix M = R_x2 * R_y * R_x1, where R_d is the cardinal rotation matrix
380         about the axis +d, rotating counterclockwise.
381         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
382         Parameters x2, y and x1 are the angles of rotation, in radians. */
383 template<typename Matrix>
384 void Set3x3PartRotateEulerXYX(Matrix &m, float &x2, float &y, float &x1)
385 {
386         ///\todo Vectorize to compute 4 sines+cosines at one time.
387         float cx2, sx2, cy, sy, cx1, sx1;
388         SinCos(x2, sx2, cx2);
389         SinCos(y, sy, cy);
390         SinCos(x1, sx1, cx1);
391
392         m[0][0] =      cy; m[0][1] =               sy*sx1; m[0][2] =                sy*cx1;
393         m[1][0] =  sy*sx2; m[1][1] = cx2*cx1 - cy*sx2*sx1; m[1][2] = -cy*cx1*sx2 - cx2*sx1;
394         m[2][0] = -sy*cx2; m[2][1] = cx1*sx2 + cy*cx2*sx1; m[2][2] =  cy*cx2*cx1 - sx2*sx1;
395 }
396
397 /** Decomposes the matrix M to form M = R_x2 * R_y * R_x1 (R_d being the cardinal rotation
398         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
399         parameters x2, y and x1, in radians.
400         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
401         The matrix m must be orthonormal. */
402 template<typename Matrix>
403 void ExtractEulerXYX(Matrix &m, float &x2, float &y, float &x1)
404 {
405         if (m[0][0] < 1.f)
406         {
407                 if (m[0][0] > -1.f)
408                 {
409                         y = acos(m[0][0]);
410                         x2 = atan2(m[1][0], -m[2][0]);
411                         x1 = atan2(m[0][1], m[0][2]);
412                 }
413                 else
414                 {
415                         // Not a unique solution.
416                         y = pi;
417                         x2 = -atan2(-m[1][2], m[1][1]);
418                         x1 = 0;
419                 }
420         }
421         else
422         {
423                 // Not a unique solution.
424                 y = 0;
425                 x2 = atan2(-m[1][2], m[1][1]);
426                 x1 = 0;
427         }
428 }
429
430 /** Computes the matrix M = R_x2 * R_z * R_x1, where R_d is the cardinal rotation matrix
431         about the axis +d, rotating counterclockwise.
432         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
433         Parameters x2, z and x1 are the angles of rotation, in radians. */
434 template<typename Matrix>
435 void Set3x3PartRotateEulerXZX(Matrix &m, float &x2, float &z, float &x1)
436 {
437         ///\todo Vectorize to compute 4 sines+cosines at one time.
438         float cx2, sx2, cz, sz, cx1, sx1;
439         SinCos(x2, sx2, cx2);
440         SinCos(z, sz, cz);
441         SinCos(x1, sx1, cx1);
442
443         m[0][0] =     cz; m[0][1] =              -sz*cx1; m[0][2] =                sz*sx1;
444         m[1][0] = sz*cx2; m[1][1] = cz*cx2*cx1 - sx2*sx1; m[1][2] = -cx1*sx2 - cz*cx2*sx1;
445         m[2][0] = sz*sx2; m[2][1] = cz*cx1*sx2 + cx2*sx1; m[2][2] =  cx2*cx1 - cz*sx2*sx1;
446 }
447
448 /** Decomposes the matrix M to form M = R_x2 * R_z * R_x1 (R_d being the cardinal rotation
449         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
450         parameters x2, z and x1, in radians.
451         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
452         The matrix m must be orthonormal. */
453 template<typename Matrix>
454 void ExtractEulerXZX(Matrix &m, float &x2, float &z, float &x1)
455 {
456         if (m[0][0] < 1.f)
457         {
458                 if (m[0][0] > -1.f)
459                 {
460                         z = acos(m[0][0]);
461                         x2 = atan2(m[2][0], m[1][0]);
462                         x1 = atan2(m[0][2], -m[0][1]);
463                 }
464                 else
465                 {
466                         // Not a unique solution.
467                         z = pi;
468                         x2 = -atan2(-m[2][1], m[2][2]);
469                         x1 = 0;
470                 }
471         }
472         else
473         {
474                 // Not a unique solution.
475                 z = 0;
476                 x2 = atan2(m[2][1], m[2][2]);
477                 x1 = 0;
478         }
479 }
480
481 /** Computes the matrix M = R_y2 * R_x * R_y1, where R_d is the cardinal rotation matrix
482         about the axis +d, rotating counterclockwise.
483         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
484         Parameters y2, x and y1 are the angles of rotation, in radians. */
485 template<typename Matrix>
486 void Set3x3PartRotateEulerYXY(Matrix &m, float &y2, float &x, float &y1)
487 {
488         ///\todo Vectorize to compute 4 sines+cosines at one time.
489         float cy2, sy2, cx, sx, cy1, sy1;
490         SinCos(y2, sy2, cy2);
491         SinCos(x, sx, cx);
492         SinCos(y1, sy1, cy1);
493
494         m[0][0] =  cy2*cy1 - cx*sy2*sy1; m[0][1] = sx*sy2; m[0][2] = cx*cy1*sy2 + cy2*sy1;
495         m[1][0] =                sx*sy1; m[1][1] =     cx; m[1][2] =              -sx*cy1;
496         m[2][0] = -cy1*sy2 - cx*cy2*sy1; m[2][1] = sx*cy2; m[2][2] = cx*cy2*cy1 - sy2*sy1;
497 }
498
499 /** Decomposes the matrix M to form M = R_y2 * R_x * R_y1 (R_d being the cardinal rotation
500         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
501         parameters y2, x and y1, in radians.
502         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
503         The matrix m must be orthonormal. */
504 template<typename Matrix>
505 void ExtractEulerYXY(Matrix &m, float &y2, float &x, float &y1)
506 {
507         if (m[1][1] < 1.f)
508         {
509                 if (m[1][1] > -1.f)
510                 {
511                         x = acos(m[1][1]);
512                         y2 = atan2(m[0][1], m[2][1]);
513                         y1 = atan2(m[1][0], -m[1][2]);
514                 }
515                 else
516                 {
517                         // Not a unique solution.
518                         x = pi;
519                         y2 = -atan2(m[0][2], m[0][0]);
520                         y1 = 0;
521                 }
522         }
523         else
524         {
525                 // Not a unique solution.
526                 x = 0;
527                 y2 = atan2(m[2][0], m[0][0]);
528                 y1 = 0;
529         }
530 }
531
532 /** Computes the matrix M = R_y2 * R_z * R_y1, where R_d is the cardinal rotation matrix
533         about the axis +d, rotating counterclockwise.
534         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
535         Parameters y2, z and y1 are the angles of rotation, in radians. */
536 template<typename Matrix>
537 void Set3x3PartRotateEulerYZY(Matrix &m, float &y2, float &z, float &y1)
538 {
539         ///\todo Vectorize to compute 4 sines+cosines at one time.
540         float cy2, sy2, cz, sz, cy1, sy1;
541         SinCos(y2, sy2, cy2);
542         SinCos(z, sz, cz);
543         SinCos(y1, sy1, cy1);
544
545         m[0][0] =  cz*cy2*cy1 - sy2*sy1; m[0][1] = -sz*cy2; m[0][2] = cy1*sy2 + cz*cy2*sy1;
546         m[1][0] =                sz*cy1; m[1][1] =      cz; m[1][2] =               sz*sy1;
547         m[2][0] = -cz*cy1*sy2 - cy2*sy1; m[2][1] =  sz*sy2; m[2][2] = cy2*cy1 - cz*sy2*sy1;
548 }
549
550 /** Decomposes the matrix M to form M = R_y2 * R_z * R_y1 (R_d being the cardinal rotation
551         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
552         parameters y2, z and y1, in radians.
553         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
554         The matrix m must be orthonormal. */
555 template<typename Matrix>
556 void ExtractEulerYZY(Matrix &m, float &y2, float &z, float &y1)
557 {
558         if (m[1][1] < 1.f)
559         {
560                 if (m[1][1] > -1.f)
561                 {
562                         z = acos(m[1][1]);
563                         y2 = atan2(m[2][1], -m[0][1]);
564                         y1 = atan2(m[1][2], m[1][0]);
565                 }
566                 else
567                 {
568                         // Not a unique solution.
569                         z = pi;
570                         y2 = -atan2(-m[2][0], m[2][2]);
571                         y1 = 0;
572                 }
573         }
574         else
575         {
576                 // Not a unique solution.
577                 z = 0;
578                 y2 = atan2(-m[2][0], m[2][2]);
579                 y1 = 0;
580         }
581 }
582
583 /** Computes the matrix M = R_z2 * R_x * R_z1, where R_d is the cardinal rotation matrix
584         about the axis +d, rotating counterclockwise.
585         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
586         Parameters z2, x and z1 are the angles of rotation, in radians. */
587 template<typename Matrix>
588 void Set3x3PartRotateEulerZXZ(Matrix &m, float &z2, float &x, float &z1)
589 {
590         ///\todo Vectorize to compute 4 sines+cosines at one time.
591         float cz2, sz2, cx, sx, cz1, sz1;
592         SinCos(z2, sz2, cz2);
593         SinCos(x, sx, cx);
594         SinCos(z1, sz1, cz1);
595
596         m[0][0] = cz2*cz1 - cx*sz2*sz1; m[0][1] = -cx*cz1*sz2 - cz2*sz1; m[0][2] =  sx*sz2;
597         m[1][0] = cz1*sz2 + cx*cz2*sz1; m[1][1] =  cx*cz2*cz1 - sz2*sz1; m[1][2] = -sx*cz2;
598         m[2][0] =               sx*sz1; m[2][1] =                sx*cz1; m[2][2] =      cx;
599 }
600
601 /** Decomposes the matrix M to form M = R_z2 * R_x * R_z1 (R_d being the cardinal rotation
602         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
603         parameters z2, x and z1, in radians.
604         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
605         The matrix m must be orthonormal. */
606 template<typename Matrix>
607 void ExtractEulerZXZ(Matrix &m, float &z2, float &x, float &z1)
608 {
609         if (m[2][2] < 1.f)
610         {
611                 if (m[2][2] > -1.f)
612                 {
613                         x = acos(m[2][2]);
614                         z2 = atan2(m[0][2], -m[1][2]);
615                         z1 = atan2(m[2][0], m[2][1]);
616                 }
617                 else
618                 {
619                         // Not a unique solution.
620                         x = pi;
621                         z2 = -atan2(-m[0][1], m[0][0]);
622                         z1 = 0;
623                 }
624         }
625         else
626         {
627                 // Not a unique solution.
628                 x = 0;
629                 z2 = atan2(-m[0][1], m[0][0]);
630                 z1 = 0;
631         }
632 }
633
634 /** Computes the matrix M = R_z2 * R_y * R_z1, where R_d is the cardinal rotation matrix
635         about the axis +d, rotating counterclockwise.
636         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf .
637         Parameters z2, y and z1 are the angles of rotation, in radians. */
638 template<typename Matrix>
639 void Set3x3PartRotateEulerZYZ(Matrix &m, float &z2, float &y, float &z1)
640 {
641         ///\todo Vectorize to compute 4 sines+cosines at one time.
642         float cz2, sz2, cy, sy, cz1, sz1;
643         SinCos(z2, sz2, cz2);
644         SinCos(y, sy, cy);
645         SinCos(z1, sz1, cz1);
646
647         m[0][0] = cy*cz2*cz1 - sz2*sz1; m[0][1] = -cz1*sz2 - cy*cz2*sz1; m[0][2] = sy*cz2;
648         m[1][0] =           cy*cz1*sz2; m[1][1] =  cz2*cz1 - cy*sz2*sz1; m[1][2] = sy*sz2;
649         m[2][0] =              -sy*cz1; m[2][1] =                sy*sz1; m[2][2] =     cy;
650 }
651
652 /** Decomposes the matrix M to form M = R_z2 * R_y * R_z1 (R_d being the cardinal rotation
653         matrix about the axis +d, rotating counterclockwise), and outputs the angles of rotation in
654         parameters z2, y and z1, in radians.
655         This function was adapted from http://www.geometrictools.com/Documentation/EulerAngles.pdf
656         The matrix m must be orthonormal. */
657 template<typename Matrix>
658 void ExtractEulerZYZ(Matrix &m, float &z2, float &y, float &z1)
659 {
660         if (m[2][2] < 1.f)
661         {
662                 if (m[2][2] > -1.f)
663                 {
664                         y = acos(m[2][2]);
665                         z2 = atan2(m[1][2], m[0][2]);
666                         z1 = atan2(m[2][1], -m[2][0]);
667                 }
668                 else
669                 {
670                         // Not a unique solution.
671                         y = pi;
672                         z2 = -atan2(-m[1][0], m[1][1]);
673                         z1 = 0;
674                 }
675         }
676         else
677         {
678                 // Not a unique solution.
679                 y = 0;
680                 z2 = atan2(m[1][0], m[1][1]);
681                 z1 = 0;
682         }
683 }
684
685 /** Sets the top-left 3x3 area of the matrix to the rotation matrix about an arbitrary axis. Elements
686         outside the top-left 3x3 area are not modified.
687         @param m The matrix to store the result.
688         @param a The axis to rotate about. The axis must be normalized.
689         @param angle The rotation angle. */
690 template<typename Matrix, typename Vector>
691 void SetRotationAxis3x3(Matrix &m, const Vector &a, float angle)
692 {
693         assume(a.IsNormalized());
694         assume(MATH_NS::IsFinite(angle));
695
696         float s, c;
697         SinCos(angle, s, c);
698
699         const float c1 = 1.f - c;
700
701         m[0][0] = c+c1*a.x*a.x;
702         m[1][0] = c1*a.x*a.y+s*a.z;
703         m[2][0] = c1*a.x*a.z-s*a.y;
704
705         m[0][1] = c1*a.x*a.y-s*a.z;
706         m[1][1] = c+c1*a.y*a.y;
707         m[2][1] = c1*a.y*a.z+s*a.x;
708
709         m[0][2] = c1*a.x*a.z+s*a.y;
710         m[1][2] = c1*a.y*a.z-s*a.x;
711         m[2][2] = c+c1*a.z*a.z;
712 }
713
714 template<typename Matrix>
715 void SetIdentity3x3(Matrix &m)
716 {
717         m[0][0] = 1.f; m[0][1] = 0.f; m[0][2] = 0.f;
718         m[1][0] = 0.f; m[1][1] = 1.f; m[1][2] = 0.f;
719         m[2][0] = 0.f; m[2][1] = 0.f; m[2][2] = 1.f;
720 }
721
722 template<typename Matrix>
723 void InverseAffineMatrixNoScale(Matrix &mat)
724 {
725         Swap(mat[0][1], mat[1][0]);
726         Swap(mat[0][2], mat[2][0]);
727         Swap(mat[1][2], mat[2][1]);
728         mat.SetTranslatePart(mat.TransformDir(-mat[0][3], -mat[1][3], -mat[2][3]));
729 }
730
731 template<typename Matrix>
732 void InverseAffineMatrixUniformScale(Matrix &mat)
733 {
734         Swap(mat[0][1], mat[1][0]);
735         Swap(mat[0][2], mat[2][0]);
736         Swap(mat[1][2], mat[2][1]);
737         float scale = sqrtf(1.f / float3(mat[0][0], mat[0][1], mat[0][2]).LengthSq());
738
739         mat[0][0] *= scale; mat[0][1] *= scale; mat[0][2] *= scale;
740         mat[1][0] *= scale; mat[1][1] *= scale; mat[1][2] *= scale;
741         mat[2][0] *= scale; mat[2][1] *= scale; mat[2][2] *= scale;
742
743         mat.SetTranslatePart(mat.TransformDir(-mat[0][3], -mat[1][3], -mat[2][3]));
744 }
745
746 template<typename Matrix>
747 void InverseAffineMatrixNonuniformScale(Matrix &mat)
748 {
749         Swap(mat[0][1], mat[1][0]);
750         Swap(mat[0][2], mat[2][0]);
751         Swap(mat[1][2], mat[2][1]);
752         for(int i = 0; i < 3; ++i)
753         {
754                 float scale = sqrtf(1.f / float3(mat[i][0], mat[i][1], mat[i][2]).LengthSq());
755                 mat[i][0] *= scale;
756                 mat[i][1] *= scale;
757                 mat[i][2] *= scale;
758         }
759         mat.SetTranslatePart(mat.TransformDir(-mat[0][3], -mat[1][3], -mat[2][3]));
760 }
761
762 template<typename Matrix>
763 bool InverseMatrix(Matrix &mat, float epsilon)
764 {
765         Matrix inversed = Matrix::identity; // will contain the inverse matrix
766
767         const int nc = Min<int>(Matrix::Rows, Matrix::Cols);
768         for(int column = 0; column < nc; ++column)
769         {
770                 // find the row i with i >= j such that M has the largest absolute value.
771                 int greatest = column;
772                 float greatestVal = Abs(mat[greatest][column]);
773                 for(int i = column+1; i < Matrix::Rows; i++)
774                 {
775                         float val = Abs(mat[i][column]);
776                         if (val > greatestVal)
777                         {
778                                 greatest = i;
779                                 greatestVal = val;
780                         }
781                 }
782
783                 if (greatestVal < epsilon)
784                 {
785                         mat = inversed;
786                         return false;
787                 }
788
789                 // exchange rows
790                 if (greatest != column)
791                 {
792                         inversed.SwapRows(greatest, column);
793                         mat.SwapRows(greatest, column);
794                 }
795
796                 // multiply rows
797                 assume(!EqualAbs(mat[column][column], 0.f, epsilon));
798                 float scale = 1.f / mat[column][column];
799                 inversed.ScaleRow(column, scale);
800                 mat.ScaleRow(column, scale);
801
802                 // add rows
803                 for(int i = 0; i < column; i++)
804                 {
805                         inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
806                         mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
807                 }
808                 for(int i = column+1; i < Matrix::Rows; i++)
809                 {
810                         inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
811                         mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
812                 }
813         }
814         mat = inversed;
815
816         return true;
817 }
818
819 /** Computes the LU-decomposition on the given square matrix.
820         @return True if the decomposition was successful, false otherwise. If the return value is false, the
821                 contents of the output matrix are unspecified.*/
822 template<typename Matrix>
823 bool LUDecomposeMatrix(const Matrix &mat, Matrix &lower, Matrix &upper)
824 {
825         lower = Matrix::identity;
826         upper = Matrix::zero;
827
828         for(int i = 0; i < Matrix::Rows; ++i)
829         {
830                 for(int col = i; col < Matrix::Cols; ++col)
831                 {
832                         upper[i][col] = mat[i][col];
833                         for(int k = 0; k < i; ++k)
834                                 upper[i][col] -= lower[i][k]*upper[k][col];
835                 }
836                 for(int row = i+1; row < Matrix::Rows; ++row)
837                 {
838                         lower[row][i] = mat[row][i];
839                         for(int k = 0; k < i; ++k)
840                                 lower[row][i] -= lower[row][k]*upper[k][i];
841                         if (EqualAbs(upper[i][i], 0.f))
842                                 return false;
843                         lower[row][i] /= upper[i][i];
844                 }
845         }
846         return true;
847 }
848
849 /** Computes the Cholesky decomposition on the given square matrix *on the real domain*.
850         @return True if succesful, false otherwise. If the return value is false, the contents of the output
851                 matrix are unspecified. */
852 template<typename Matrix>
853 bool CholeskyDecomposeMatrix(const Matrix &mat, Matrix &lower)
854 {
855         lower = Matrix::zero;
856         for(int i = 0; i < Matrix::Rows; ++i)
857         {
858                 for(int j = 0; j < i; ++j)
859                 {
860                         lower[i][j] = mat[i][j];
861                         for(int k = 0; k < j; ++k)
862                                 lower[i][j] -= lower[i][k]*lower[j][k];
863                         if (EqualAbs(lower[j][j], 0.f))
864                                 return false;
865                         lower[i][j] /= lower[j][j];
866                 }
867
868                 lower[i][i] = mat[i][i];
869                 if (lower[i][i])
870                         return false;
871                 for(int k = 0; k < i; ++k)
872                         lower[i][i] -= lower[i][k]*lower[i][k];
873                 lower[i][i] = Sqrt(lower[i][i]);
874         }
875         return true;
876 }
877
878 template<typename Matrix>
879 void SetMatrixRotatePart(Matrix &m, const Quat &q)
880 {
881         // See e.g. http://www.geometrictools.com/Documentation/LinearAlgebraicQuaternions.pdf .
882
883         assume2(q.IsNormalized(1e-3f), q.ToString(), q.LengthSq());
884         const float x = q.x; const float y = q.y; const float z = q.z; const float w = q.w;
885         m[0][0] = 1 - 2*(y*y + z*z); m[0][1] =     2*(x*y - z*w); m[0][2] =     2*(x*z + y*w);
886         m[1][0] =     2*(x*y + z*w); m[1][1] = 1 - 2*(x*x + z*z); m[1][2] =     2*(y*z - x*w);
887         m[2][0] =     2*(x*z - y*w); m[2][1] =     2*(y*z + x*w); m[2][2] = 1 - 2*(x*x + y*y);
888 }
889
890 /// See e.g. http://en.wikipedia.org/wiki/Transformation_matrix#reflection
891 template<typename Matrix>
892 void SetMatrix3x3LinearPlaneMirror(Matrix &m, float x, float y, float z)
893 {
894         assume(float3(x,y,z).IsNormalized());
895         m[0][0] = 1.f - 2.f*x*x; m[0][1] =      -2.f*y*x; m[0][2] =      -2.f*z*x;
896         m[1][0] =      -2.f*x*y; m[1][1] = 1.f - 2.f*y*y; m[1][2] =      -2.f*y*z;
897         m[2][0] =      -2.f*x*z; m[2][1] =      -2.f*y*z; m[2][2] = 1.f - 2.f*z*z;
898 }
899
900 template<typename Matrix>
901 void SetMatrix3x4AffinePlaneMirror(Matrix &m, float x, float y, float z, float d)
902 {
903         SetMatrix3x3LinearPlaneMirror(m, x, y, z);
904         m[0][3] = 2.f * d * x;
905         m[1][3] = 2.f * d * y;
906         m[2][3] = 2.f * d * z;
907 }
908
909 template<typename Matrix>
910 void SetMatrix3x3LinearPlaneProject(Matrix &m, float x, float y, float z)
911 {
912         assume(float3(x,y,z).IsNormalized());
913         m[0][0] = 1.f - x*x; m[0][1] =      -y*x; m[0][2] =      -z*x;
914         m[1][0] =      -x*y; m[1][1] = 1.f - y*y; m[1][2] =      -y*z;
915         m[2][0] =      -x*z; m[2][1] =      -y*z; m[2][2] = 1.f - z*z;
916 }
917
918 template<typename Matrix>
919 void SetMatrix3x4AffinePlaneProject(Matrix &m, float x, float y, float z, float d)
920 {
921         SetMatrix3x3LinearPlaneProject(m, x, y, z);
922         m[0][3] = d * x;
923         m[1][3] = d * y;
924         m[2][3] = d * z;
925 }
926
927 MATH_END_NAMESPACE