]> git.cworth.org Git - vogl/blob - src/voglcore/vogl_matrix.h
- Fixing polygon stipple saving/restoring so it doesn't stomp memory after the polygo...
[vogl] / src / voglcore / vogl_matrix.h
1 /**************************************************************************
2  *
3  * Copyright 2013-2014 RAD Game Tools and Valve Software
4  * Copyright 2010-2014 Rich Geldreich and Tenacious Software LLC
5  * All Rights Reserved.
6  *
7  * Permission is hereby granted, free of charge, to any person obtaining a copy
8  * of this software and associated documentation files (the "Software"), to deal
9  * in the Software without restriction, including without limitation the rights
10  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11  * copies of the Software, and to permit persons to whom the Software is
12  * furnished to do so, subject to the following conditions:
13  *
14  * The above copyright notice and this permission notice shall be included in
15  * all copies or substantial portions of the Software.
16  *
17  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
18  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
19  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
20  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
21  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
22  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
23  * THE SOFTWARE.
24  *
25  **************************************************************************/
26
27 // File: vogl_matrix.h
28 #pragma once
29
30 #include "vogl_core.h"
31 #include "vogl_vec.h"
32
33 namespace vogl
34 {
35     // Matrix/vector cheat sheet, because confusingly, depending on how matrices are stored in memory people can use opposite definitions of "rows", "cols", etc.
36     // See http://www.mindcontrol.org/~hplus/graphics/matrix-layout.html
37     //
38     // So in this simple general matrix class:
39     // matrix=[NumRows][NumCols] or [R][C], i.e. a 3x3 matrix stored in memory will appear as: R0C0, R0C1, R0C2,  R1C0, R1C1, R1C2,  etc.
40     // Matrix multiplication: [R0,C0]*[R1,C1]=[R0,C1], C0 must equal R1
41     //
42     // In this class:
43     // A "row vector" type is a vector of size # of matrix cols, 1xC. It's the vector type that is used to store the matrix rows.
44     // A "col vector" type is a vector of size # of matrix rows, Rx1. It's a vector type large enough to hold each matrix column.
45     //
46     // Subrow/col vectors: last component is assumed to be either 0 (a "vector") or 1 (a "point")
47     // "subrow vector": vector/point of size # cols-1, 1x(C-1)
48     // "subcol vector": vector/point of size # rows-1, (R-1)x1
49     //
50     // D3D style:
51     // vec*matrix, row vector on left (vec dotted against columns)
52     // [1,4]*[4,4]=[1,4]
53     // abcd * A B C D
54     //        A B C D
55     //        A B C D
56     //        A B C D
57     // =      e f g h
58     //
59     // Now confusingly, in the matrix transform method for vec*matrix below the vector's type is "col_vec", because col_vec will have the proper size for non-square matrices. But the vector on the left is written as row vector, argh.
60     //
61     //
62     // OGL style:
63     // matrix*vec, col vector on right (vec dotted against rows):
64     // [4,4]*[4,1]=[4,1]
65     //
66     // A B C D * e = e
67     // A B C D   f   f
68     // A B C D   g   g
69     // A B C D   h   h
70
71     template <class X, class Y, class Z>
72     Z &matrix_mul_helper(Z &result, const X &lhs, const Y &rhs)
73     {
74         VOGL_ASSUME(Z::num_rows == X::num_rows);
75         VOGL_ASSUME(Z::num_cols == Y::num_cols);
76         VOGL_ASSUME(X::num_cols == Y::num_rows);
77         VOGL_ASSERT(((void *)&result != (void *)&lhs) && ((void *)&result != (void *)&rhs));
78         for (int r = 0; r < X::num_rows; r++)
79             for (int c = 0; c < Y::num_cols; c++)
80             {
81                 typename Z::scalar_type s = lhs(r, 0) * rhs(0, c);
82                 for (uint i = 1; i < X::num_cols; i++)
83                     s += lhs(r, i) * rhs(i, c);
84                 result(r, c) = s;
85             }
86         return result;
87     }
88
89     template <class X, class Y, class Z>
90     Z &matrix_mul_helper_transpose_lhs(Z &result, const X &lhs, const Y &rhs)
91     {
92         VOGL_ASSUME(Z::num_rows == X::num_cols);
93         VOGL_ASSUME(Z::num_cols == Y::num_cols);
94         VOGL_ASSUME(X::num_rows == Y::num_rows);
95         VOGL_ASSERT(((void *)&result != (void *)&lhs) && ((void *)&result != (void *)&rhs));
96         for (int r = 0; r < X::num_cols; r++)
97             for (int c = 0; c < Y::num_cols; c++)
98             {
99                 typename Z::scalar_type s = lhs(0, r) * rhs(0, c);
100                 for (uint i = 1; i < X::num_rows; i++)
101                     s += lhs(i, r) * rhs(i, c);
102                 result(r, c) = s;
103             }
104         return result;
105     }
106
107     template <class X, class Y, class Z>
108     Z &matrix_mul_helper_transpose_rhs(Z &result, const X &lhs, const Y &rhs)
109     {
110         VOGL_ASSUME(Z::num_rows == X::num_rows);
111         VOGL_ASSUME(Z::num_cols == Y::num_rows);
112         VOGL_ASSUME(X::num_cols == Y::num_cols);
113         VOGL_ASSERT(((void *)&result != (void *)&lhs) && ((void *)&result != (void *)&rhs));
114         for (int r = 0; r < X::num_rows; r++)
115             for (int c = 0; c < Y::num_rows; c++)
116             {
117                 typename Z::scalar_type s = lhs(r, 0) * rhs(c, 0);
118                 for (uint i = 1; i < X::num_cols; i++)
119                     s += lhs(r, i) * rhs(c, i);
120                 result(r, c) = s;
121             }
122         return result;
123     }
124
125     enum eIdentity
126     {
127         cIdentity
128     };
129
130     template <uint R, uint C, typename T>
131     class matrix
132     {
133     public:
134         typedef T scalar_type;
135         enum
136         {
137             num_rows = R,
138             num_cols = C
139         };
140
141         typedef vec<R, T> col_vec;
142         typedef vec < (R > 1) ? (R - 1) : 0, T > subcol_vec;
143
144         typedef vec<C, T> row_vec;
145         typedef vec < (C > 1) ? (C - 1) : 0, T > subrow_vec;
146
147         inline matrix()
148         {
149         }
150
151         inline matrix(eClear)
152         {
153             clear();
154         }
155
156         inline matrix(eIdentity)
157         {
158             set_identity_matrix();
159         }
160
161         inline matrix(const T *p)
162         {
163             set(p);
164         }
165
166         inline matrix(const matrix &other)
167         {
168             for (uint i = 0; i < R; i++)
169                 m_rows[i] = other.m_rows[i];
170         }
171
172         inline matrix &operator=(const matrix &rhs)
173         {
174             if (this != &rhs)
175                 for (uint i = 0; i < R; i++)
176                     m_rows[i] = rhs.m_rows[i];
177             return *this;
178         }
179
180         inline matrix(T val00, T val01,
181                       T val10, T val11)
182         {
183             set(val00, val01, val10, val11);
184         }
185
186         inline matrix(T val00, T val01,
187                       T val10, T val11,
188                       T val20, T val21)
189         {
190             set(val00, val01, val10, val11, val20, val21);
191         }
192
193         inline matrix(T val00, T val01, T val02,
194                       T val10, T val11, T val12,
195                       T val20, T val21, T val22)
196         {
197             set(val00, val01, val02, val10, val11, val12, val20, val21, val22);
198         }
199
200         inline matrix(T val00, T val01, T val02, T val03,
201                       T val10, T val11, T val12, T val13,
202                       T val20, T val21, T val22, T val23,
203                       T val30, T val31, T val32, T val33)
204         {
205             set(val00, val01, val02, val03, val10, val11, val12, val13, val20, val21, val22, val23, val30, val31, val32, val33);
206         }
207
208         inline matrix(T val00, T val01, T val02, T val03,
209                       T val10, T val11, T val12, T val13,
210                       T val20, T val21, T val22, T val23)
211         {
212             set(val00, val01, val02, val03, val10, val11, val12, val13, val20, val21, val22, val23);
213         }
214
215         inline void set(const float *p)
216         {
217             for (uint i = 0; i < R; i++)
218             {
219                 m_rows[i].set(p);
220                 p += C;
221             }
222         }
223
224         inline void set(T val00, T val01,
225                         T val10, T val11)
226         {
227             m_rows[0].set(val00, val01);
228             if (R >= 2)
229             {
230                 m_rows[1].set(val10, val11);
231
232                 for (uint i = 2; i < R; i++)
233                     m_rows[i].clear();
234             }
235         }
236
237         inline void set(T val00, T val01,
238                         T val10, T val11,
239                         T val20, T val21)
240         {
241             m_rows[0].set(val00, val01);
242             if (R >= 2)
243             {
244                 m_rows[1].set(val10, val11);
245
246                 if (R >= 3)
247                 {
248                     m_rows[2].set(val20, val21);
249
250                     for (uint i = 3; i < R; i++)
251                         m_rows[i].clear();
252                 }
253             }
254         }
255
256         inline void set(T val00, T val01, T val02,
257                         T val10, T val11, T val12,
258                         T val20, T val21, T val22)
259         {
260             m_rows[0].set(val00, val01, val02);
261             if (R >= 2)
262             {
263                 m_rows[1].set(val10, val11, val12);
264                 if (R >= 3)
265                 {
266                     m_rows[2].set(val20, val21, val22);
267
268                     for (uint i = 3; i < R; i++)
269                         m_rows[i].clear();
270                 }
271             }
272         }
273
274         inline void set(T val00, T val01, T val02, T val03,
275                         T val10, T val11, T val12, T val13,
276                         T val20, T val21, T val22, T val23,
277                         T val30, T val31, T val32, T val33)
278         {
279             m_rows[0].set(val00, val01, val02, val03);
280             if (R >= 2)
281             {
282                 m_rows[1].set(val10, val11, val12, val13);
283                 if (R >= 3)
284                 {
285                     m_rows[2].set(val20, val21, val22, val23);
286
287                     if (R >= 4)
288                     {
289                         m_rows[3].set(val30, val31, val32, val33);
290
291                         for (uint i = 4; i < R; i++)
292                             m_rows[i].clear();
293                     }
294                 }
295             }
296         }
297
298         inline void set(T val00, T val01, T val02, T val03,
299                         T val10, T val11, T val12, T val13,
300                         T val20, T val21, T val22, T val23)
301         {
302             m_rows[0].set(val00, val01, val02, val03);
303             if (R >= 2)
304             {
305                 m_rows[1].set(val10, val11, val12, val13);
306                 if (R >= 3)
307                 {
308                     m_rows[2].set(val20, val21, val22, val23);
309
310                     for (uint i = 3; i < R; i++)
311                         m_rows[i].clear();
312                 }
313             }
314         }
315
316         inline T operator()(uint r, uint c) const
317         {
318             VOGL_ASSERT((r < R) && (c < C));
319             return m_rows[r][c];
320         }
321
322         inline T &operator()(uint r, uint c)
323         {
324             VOGL_ASSERT((r < R) && (c < C));
325             return m_rows[r][c];
326         }
327
328         inline const row_vec &operator[](uint r) const
329         {
330             VOGL_ASSERT(r < R);
331             return m_rows[r];
332         }
333
334         inline row_vec &operator[](uint r)
335         {
336             VOGL_ASSERT(r < R);
337             return m_rows[r];
338         }
339
340         inline const row_vec &get_row(uint r) const
341         {
342             return (*this)[r];
343         }
344         inline row_vec &get_row(uint r)
345         {
346             return (*this)[r];
347         }
348
349         inline col_vec get_col(uint c) const
350         {
351             VOGL_ASSERT(c < C);
352             col_vec result;
353             for (uint i = 0; i < R; i++)
354                 result[i] = m_rows[i][c];
355             return result;
356         }
357
358         inline void set_col(uint c, const col_vec &col)
359         {
360             VOGL_ASSERT(c < C);
361             for (uint i = 0; i < R; i++)
362                 m_rows[i][c] = col[i];
363         }
364
365         inline void set_col(uint c, const subcol_vec &col)
366         {
367             VOGL_ASSERT(c < C);
368             for (uint i = 0; i < (R - 1); i++)
369                 m_rows[i][c] = col[i];
370
371             m_rows[R - 1][c] = 0.0f;
372         }
373
374         inline const row_vec &get_translate() const
375         {
376             return m_rows[R - 1];
377         }
378
379         inline matrix &set_translate(const row_vec &r)
380         {
381             m_rows[R - 1] = r;
382             return *this;
383         }
384
385         inline matrix &set_translate(const subrow_vec &r)
386         {
387             m_rows[R - 1] = row_vec(r).as_point();
388             return *this;
389         }
390
391         inline const T *get_ptr() const
392         {
393             return reinterpret_cast<const T *>(&m_rows[0]);
394         }
395         inline T *get_ptr()
396         {
397             return reinterpret_cast<T *>(&m_rows[0]);
398         }
399
400         inline matrix &operator+=(const matrix &other)
401         {
402             for (uint i = 0; i < R; i++)
403                 m_rows[i] += other.m_rows[i];
404             return *this;
405         }
406
407         inline matrix &operator-=(const matrix &other)
408         {
409             for (uint i = 0; i < R; i++)
410                 m_rows[i] -= other.m_rows[i];
411             return *this;
412         }
413
414         inline matrix &operator*=(T val)
415         {
416             for (uint i = 0; i < R; i++)
417                 m_rows[i] *= val;
418             return *this;
419         }
420
421         inline matrix &operator/=(T val)
422         {
423             for (uint i = 0; i < R; i++)
424                 m_rows[i] /= val;
425             return *this;
426         }
427
428         inline matrix &operator*=(const matrix &other)
429         {
430             matrix result;
431             matrix_mul_helper(result, *this, other);
432             *this = result;
433             return *this;
434         }
435
436         friend inline matrix operator+(const matrix &lhs, const matrix &rhs)
437         {
438             matrix result;
439             for (uint i = 0; i < R; i++)
440                 result[i] = lhs.m_rows[i] + rhs.m_rows[i];
441             return result;
442         }
443
444         friend inline matrix operator-(const matrix &lhs, const matrix &rhs)
445         {
446             matrix result;
447             for (uint i = 0; i < R; i++)
448                 result[i] = lhs.m_rows[i] - rhs.m_rows[i];
449             return result;
450         }
451
452         friend inline matrix operator*(const matrix &lhs, T val)
453         {
454             matrix result;
455             for (uint i = 0; i < R; i++)
456                 result[i] = lhs.m_rows[i] * val;
457             return result;
458         }
459
460         friend inline matrix operator/(const matrix &lhs, T val)
461         {
462             matrix result;
463             for (uint i = 0; i < R; i++)
464                 result[i] = lhs.m_rows[i] / val;
465             return result;
466         }
467
468         friend inline matrix operator*(T val, const matrix &rhs)
469         {
470             matrix result;
471             for (uint i = 0; i < R; i++)
472                 result[i] = val * rhs.m_rows[i];
473             return result;
474         }
475
476         friend inline matrix operator*(const matrix &lhs, const matrix &rhs)
477         {
478             matrix result;
479             return matrix_mul_helper(result, lhs, rhs);
480         }
481
482         friend inline row_vec operator*(const col_vec &a, const matrix &b)
483         {
484             return transform(a, b);
485         }
486
487         inline matrix operator+() const
488         {
489             return *this;
490         }
491
492         inline matrix operator-() const
493         {
494             matrix result;
495             for (uint i = 0; i < R; i++)
496                 result[i] = -m_rows[i];
497             return result;
498         }
499
500         inline matrix &clear()
501         {
502             for (uint i = 0; i < R; i++)
503                 m_rows[i].clear();
504             return *this;
505         }
506
507         inline matrix &set_zero_matrix()
508         {
509             clear();
510             return *this;
511         }
512
513         inline matrix &set_identity_matrix()
514         {
515             for (uint i = 0; i < R; i++)
516             {
517                 m_rows[i].clear();
518                 m_rows[i][i] = 1.0f;
519             }
520             return *this;
521         }
522
523         inline matrix &set_scale_matrix(float s)
524         {
525             clear();
526             for (int i = 0; i < (R - 1); i++)
527                 m_rows[i][i] = s;
528             m_rows[R - 1][C - 1] = 1.0f;
529             return *this;
530         }
531
532         inline matrix &set_scale_matrix(const row_vec &s)
533         {
534             clear();
535             for (uint i = 0; i < R; i++)
536                 m_rows[i][i] = s[i];
537             return *this;
538         }
539
540         inline matrix &set_scale_matrix(float x, float y)
541         {
542             set_identity_matrix();
543             m_rows[0].set_x(x);
544             m_rows[1].set_y(y);
545             return *this;
546         }
547
548         inline matrix &set_scale_matrix(float x, float y, float z)
549         {
550             set_identity_matrix();
551             m_rows[0].set_x(x);
552             m_rows[1].set_y(y);
553             m_rows[2].set_z(z);
554             return *this;
555         }
556
557         inline matrix &set_translate_matrix(const row_vec &s)
558         {
559             set_identity_matrix();
560             set_translate(s);
561             return *this;
562         }
563
564         inline matrix &set_translate_matrix(float x, float y)
565         {
566             set_identity_matrix();
567             set_translate(row_vec(x, y).as_point());
568             return *this;
569         }
570
571         inline matrix &set_translate_matrix(float x, float y, float z)
572         {
573             set_identity_matrix();
574             set_translate(row_vec(x, y, z).as_point());
575             return *this;
576         }
577
578         inline matrix get_transposed() const
579         {
580             matrix result;
581             for (uint i = 0; i < R; i++)
582                 for (uint j = 0; j < C; j++)
583                     result.m_rows[i][j] = m_rows[j][i];
584             return result;
585         }
586
587         inline matrix &transpose_in_place()
588         {
589             matrix result;
590             for (uint i = 0; i < R; i++)
591                 for (uint j = 0; j < C; j++)
592                     result.m_rows[i][j] = m_rows[j][i];
593             *this = result;
594             return *this;
595         }
596
597         bool invert(matrix &result) const
598         {
599             VOGL_ASSUME(R == C);
600
601             result.set_identity_matrix();
602
603             matrix mat(*this);
604
605             for (uint c = 0; c < C; c++)
606             {
607                 uint max_r = c;
608                 for (uint r = c + 1; r < R; r++)
609                     if (fabs(mat[r][c]) > fabs(mat[max_r][c]))
610                         max_r = r;
611
612                 if (mat[max_r][c] == 0.0f)
613                 {
614                     result.set_identity_matrix();
615                     return false;
616                 }
617
618                 utils::swap(mat[c], mat[max_r]);
619                 utils::swap(result[c], result[max_r]);
620
621                 result[c] /= mat[c][c];
622                 mat[c] /= mat[c][c];
623
624                 for (uint row = 0; row < R; row++)
625                 {
626                     if (row != c)
627                     {
628                         const row_vec temp(mat[row][c]);
629                         mat[row] -= row_vec::mul_components(mat[c], temp);
630                         result[row] -= row_vec::mul_components(result[c], temp);
631                     }
632                 }
633             }
634
635             return true;
636         }
637
638         matrix &invert_in_place()
639         {
640             matrix result;
641             invert(result);
642             *this = result;
643             return *this;
644         }
645
646         matrix get_inverse() const
647         {
648             matrix result;
649             invert(result);
650             return result;
651         }
652
653         T get_det() const
654         {
655             VOGL_ASSERT(R == C);
656             return det_helper(*this, R);
657         }
658
659         bool equal_tol(const matrix &b, float tol) const
660         {
661             for (uint r = 0; r < R; r++)
662                 if (!row_vec::equal_tol(m_rows[r], b.m_rows[r], tol))
663                     return false;
664             return true;
665         }
666
667         // This method transforms a vec by a matrix (D3D-style: row vector on left).
668         // Confusingly, note that the data type is named "col_vec", but mathematically it's actually written as a row vector (of size equal to the # matrix rows, which is why it's called a "col_vec" in this class).
669         // 1xR * RxC = 1xC
670         // This dots against the matrix columns.
671         static inline row_vec transform(const col_vec &a, const matrix &b)
672         {
673             row_vec result(b[0] * a[0]);
674             for (uint r = 1; r < R; r++)
675                 result += b[r] * a[r];
676             return result;
677         }
678
679         // This method transforms a vec by a matrix (D3D-style: row vector on left).
680         // Last component of vec is assumed to be 1.
681         static inline row_vec transform_point(const col_vec &a, const matrix &b)
682         {
683             row_vec result(0);
684             for (int r = 0; r < (R - 1); r++)
685                 result += b[r] * a[r];
686             result += b[R - 1];
687             return result;
688         }
689
690         // This method transforms a vec by a matrix (D3D-style: row vector on left).
691         // Last component of vec is assumed to be 0.
692         static inline row_vec transform_vector(const col_vec &a, const matrix &b)
693         {
694             row_vec result(0);
695             for (int r = 0; r < (R - 1); r++)
696                 result += b[r] * a[r];
697             return result;
698         }
699
700         // This method transforms a vec by a matrix (D3D-style: row vector on left).
701         // Last component of vec is assumed to be 1.
702         static inline subcol_vec transform_point(const subcol_vec &a, const matrix &b)
703         {
704             subcol_vec result(0);
705             for (int r = 0; r < static_cast<int>(R); r++)
706             {
707                 const T s = (r < subcol_vec::num_elements) ? a[r] : 1.0f;
708                 for (int c = 0; c < static_cast<int>(C - 1); c++)
709                     result[c] += b[r][c] * s;
710             }
711             return result;
712         }
713
714         // This method transforms a vec by a matrix (D3D-style: row vector on left).
715         // Last component of vec is assumed to be 0.
716         static inline subcol_vec transform_vector(const subcol_vec &a, const matrix &b)
717         {
718             subcol_vec result(0);
719             for (int r = 0; r < static_cast<int>(R - 1); r++)
720             {
721                 const T s = a[r];
722                 for (int c = 0; c < static_cast<int>(C - 1); c++)
723                     result[c] += b[r][c] * s;
724             }
725             return result;
726         }
727
728         // Like transform() above, but the matrix is effectively transposed before the multiply.
729         static inline col_vec transform_transposed(const col_vec &a, const matrix &b)
730         {
731             VOGL_ASSUME(R == C);
732             col_vec result;
733             for (uint r = 0; r < R; r++)
734                 result[r] = b[r].dot(a);
735             return result;
736         }
737
738         // Like transform() above, but the matrix is effectively transposed before the multiply.
739         // Last component of vec is assumed to be 0.
740         static inline col_vec transform_vector_transposed(const col_vec &a, const matrix &b)
741         {
742             VOGL_ASSUME(R == C);
743             col_vec result;
744             for (uint r = 0; r < R; r++)
745             {
746                 T s = 0;
747                 for (uint c = 0; c < (C - 1); c++)
748                     s += b[r][c] * a[c];
749
750                 result[r] = s;
751             }
752             return result;
753         }
754
755         // This method transforms a vec by a matrix (D3D-style: row vector on left), but the matrix is effectively transposed before the multiply.
756         // Last component of vec is assumed to be 1.
757         static inline subcol_vec transform_point_transposed(const subcol_vec &a, const matrix &b)
758         {
759             VOGL_ASSUME(R == C);
760             subcol_vec result(0);
761             for (int r = 0; r < R; r++)
762             {
763                 const T s = (r < subcol_vec::num_elements) ? a[r] : 1.0f;
764                 for (int c = 0; c < (C - 1); c++)
765                     result[c] += b[c][r] * s;
766             }
767             return result;
768         }
769
770         // This method transforms a vec by a matrix (D3D-style: row vector on left), but the matrix is effectively transposed before the multiply.
771         // Last component of vec is assumed to be 0.
772         static inline subcol_vec transform_vector_transposed(const subcol_vec &a, const matrix &b)
773         {
774             VOGL_ASSUME(R == C);
775             subcol_vec result(0);
776             for (int r = 0; r < static_cast<int>(R - 1); r++)
777             {
778                 const T s = a[r];
779                 for (int c = 0; c < static_cast<int>(C - 1); c++)
780                     result[c] += b[c][r] * s;
781             }
782             return result;
783         }
784
785         // This method transforms a matrix by a vector (OGL style, col vector on the right).
786         // Note that the data type is named "row_vec", but mathematically it's actually written as a column vector (of size equal to the # matrix cols).
787         // RxC * Cx1 = Rx1
788         // This dots against the matrix rows.
789         static inline col_vec transform(const matrix &b, const row_vec &a)
790         {
791             col_vec result;
792             for (int r = 0; r < static_cast<int>(R); r++)
793                 result[r] = b[r].dot(a);
794             return result;
795         }
796
797         // This method transforms a matrix by a vector (OGL style, col vector on the right), except the matrix is effectively transposed before the multiply.
798         // Note that the data type is named "row_vec", but mathematically it's actually written as a column vector (of size equal to the # matrix cols).
799         // RxC * Cx1 = Rx1
800         // This dots against the matrix cols.
801         static inline col_vec transform_transposed(const matrix &b, const row_vec &a)
802         {
803             VOGL_ASSUME(R == C);
804             row_vec result(b[0] * a[0]);
805             for (int r = 1; r < static_cast<int>(R); r++)
806                 result += b[r] * a[r];
807             return col_vec(result);
808         }
809
810         static inline matrix &mul_components(matrix &result, const matrix &lhs, const matrix &rhs)
811         {
812             for (uint r = 0; r < R; r++)
813                 result[r] = row_vec::mul_components(lhs[r], rhs[r]);
814             return result;
815         }
816
817         static inline matrix &concat(matrix &lhs, const matrix &rhs)
818         {
819             return matrix_mul_helper(lhs, matrix(lhs), rhs);
820         }
821
822         inline matrix &concat_in_place(const matrix &rhs)
823         {
824             return concat(*this, rhs);
825         }
826
827         static inline matrix &multiply(matrix &result, const matrix &lhs, const matrix &rhs)
828         {
829             matrix temp;
830             matrix *pResult = ((&result == &lhs) || (&result == &rhs)) ? &temp : &result;
831
832             matrix_mul_helper(*pResult, lhs, rhs);
833             if (pResult != &result)
834                 result = *pResult;
835
836             return result;
837         }
838
839         static matrix make_zero_matrix()
840         {
841             matrix result;
842             result.clear();
843             return result;
844         }
845
846         static matrix make_identity_matrix()
847         {
848             matrix result;
849             result.set_identity_matrix();
850             return result;
851         }
852
853         static matrix make_translate_matrix(const row_vec &t)
854         {
855             return matrix(cIdentity).set_translate(t);
856         }
857
858         static matrix make_translate_matrix(float x, float y)
859         {
860             return matrix(cIdentity).set_translate_matrix(x, y);
861         }
862
863         static matrix make_translate_matrix(float x, float y, float z)
864         {
865             return matrix(cIdentity).set_translate_matrix(x, y, z);
866         }
867
868         static inline matrix make_scale_matrix(float s)
869         {
870             return matrix().set_scale_matrix(s);
871         }
872
873         static inline matrix make_scale_matrix(const row_vec &s)
874         {
875             return matrix().set_scale_matrix(s);
876         }
877
878         static inline matrix make_scale_matrix(float x, float y)
879         {
880             VOGL_ASSUME(R >= 3 && C >= 3);
881             matrix result;
882             result.set_identity_matrix();
883             result.m_rows[0][0] = x;
884             result.m_rows[1][1] = y;
885             return result;
886         }
887
888         static inline matrix make_scale_matrix(float x, float y, float z)
889         {
890             VOGL_ASSUME(R >= 4 && C >= 4);
891             matrix result;
892             result.set_identity_matrix();
893             result.m_rows[0][0] = x;
894             result.m_rows[1][1] = y;
895             result.m_rows[2][2] = z;
896             return result;
897         }
898
899         // Helpers derived from Graphics Gems 1 and 2 (Matrices and Transformations, Ronald N. Goldman)
900         static matrix make_rotate_matrix(const vec<3, T> &axis, T ang)
901         {
902             VOGL_ASSUME(R >= 3 && C >= 3);
903
904             vec<3, T> norm_axis(axis.get_normalized());
905
906             double cos_a = cos(ang);
907             double inv_cos_a = 1.0f - cos_a;
908
909             double sin_a = sin(ang);
910
911             const T x = norm_axis[0];
912             const T y = norm_axis[1];
913             const T z = norm_axis[2];
914
915             const double x2 = norm_axis[0] * norm_axis[0];
916             const double y2 = norm_axis[1] * norm_axis[1];
917             const double z2 = norm_axis[2] * norm_axis[2];
918
919             matrix result;
920             result.set_identity_matrix();
921
922             result[0][0] = (T)((inv_cos_a * x2) + cos_a);
923             result[1][0] = (T)((inv_cos_a * x * y) + (sin_a * z));
924             result[2][0] = (T)((inv_cos_a * x * z) - (sin_a * y));
925
926             result[0][1] = (T)((inv_cos_a * x * y) - (sin_a * z));
927             result[1][1] = (T)((inv_cos_a * y2) + cos_a);
928             result[2][1] = (T)((inv_cos_a * y * z) + (sin_a * x));
929
930             result[0][2] = (T)((inv_cos_a * x * z) + (sin_a * y));
931             result[1][2] = (T)((inv_cos_a * y * z) - (sin_a * x));
932             result[2][2] = (T)((inv_cos_a * z2) + cos_a);
933
934             return result;
935         }
936
937         static inline matrix make_rotate_matrix(T ang)
938         {
939             VOGL_ASSUME(R >= 2 && C >= 2);
940
941             matrix ret(cIdentity);
942
943             const T sin_a = static_cast<T>(sin(ang));
944             const T cos_a = static_cast<T>(cos(ang));
945
946             ret[0][0] = +cos_a;
947             ret[0][1] = -sin_a;
948             ret[1][0] = +sin_a;
949             ret[1][1] = +cos_a;
950
951             return ret;
952         }
953
954         static inline matrix make_rotate_matrix(uint axis, T ang)
955         {
956             vec<3, T> axis_vec;
957             axis_vec.clear();
958             axis_vec[axis] = 1.0f;
959             return make_rotate_matrix(axis_vec, ang);
960         }
961
962         static inline matrix make_cross_product_matrix(const vec<3, scalar_type> &c)
963         {
964             VOGL_ASSUME((num_rows >= 3) && (num_cols >= 3));
965             matrix ret(cClear);
966             ret[0][1] = c[2];
967             ret[0][2] = -c[1];
968             ret[1][0] = -c[2];
969             ret[1][2] = c[0];
970             ret[2][0] = c[1];
971             ret[2][1] = -c[0];
972             return ret;
973         }
974
975         static inline matrix make_reflection_matrix(const vec<4, scalar_type> &n, const vec<4, scalar_type> &q)
976         {
977             VOGL_ASSUME((num_rows == 4) && (num_cols == 4));
978             matrix ret;
979             VOGL_ASSERT(n.is_vector() && q.is_vector());
980             ret = make_identity_matrix() - 2.0f * make_tensor_product_matrix(n, n);
981             ret.set_translate((2.0f * q.dot(n) * n).as_point());
982             return ret;
983         }
984
985         static inline matrix make_tensor_product_matrix(const row_vec &v, const row_vec &w)
986         {
987             matrix ret;
988             for (int r = 0; r < num_rows; r++)
989                 ret[r] = row_vec::mul_components(v.broadcast(r), w);
990             return ret;
991         }
992
993         static inline matrix make_uniform_scaling_matrix(const vec<4, scalar_type> &q, scalar_type c)
994         {
995             VOGL_ASSUME((num_rows == 4) && (num_cols == 4));
996             VOGL_ASSERT(q.is_vector());
997             matrix ret;
998             ret = c * make_identity_matrix();
999             ret.set_translate(((1.0f - c) * q).as_point());
1000             return ret;
1001         }
1002
1003         static inline matrix make_nonuniform_scaling_matrix(const vec<4, scalar_type> &q, scalar_type c, const vec<4, scalar_type> &w)
1004         {
1005             VOGL_ASSUME((num_rows == 4) && (num_cols == 4));
1006             VOGL_ASSERT(q.is_vector() && w.is_vector());
1007             matrix ret;
1008             ret = make_identity_matrix() - (1.0f - c) * make_tensor_product_matrix(w, w);
1009             ret.set_translate(((1.0f - c) * q.dot(w) * w).as_point());
1010             return ret;
1011         }
1012
1013         // n = normal of plane, q = point on plane
1014         static inline matrix make_ortho_projection_matrix(const vec<4, scalar_type> &n, const vec<4, scalar_type> &q)
1015         {
1016             VOGL_ASSERT(n.is_vector() && q.is_vector());
1017             matrix ret;
1018             ret = make_identity_matrix() - make_tensor_product_matrix(n, n);
1019             ret.set_translate((q.dot(n) * n).as_point());
1020             return ret;
1021         }
1022
1023         static inline matrix make_parallel_projection(const vec<4, scalar_type> &n, const vec<4, scalar_type> &q, const vec<4, scalar_type> &w)
1024         {
1025             VOGL_ASSERT(n.is_vector() && q.is_vector() && w.is_vector());
1026             matrix ret;
1027             ret = make_identity_matrix() - (make_tensor_product_matrix(n, w) / (w.dot(n)));
1028             ret.set_translate(((q.dot(n) / w.dot(n)) * w).as_point());
1029             return ret;
1030         }
1031
1032     protected:
1033         row_vec m_rows[R];
1034
1035         static T det_helper(const matrix &a, uint n)
1036         {
1037             // Algorithm ported from Numerical Recipes in C.
1038             T d;
1039             matrix m;
1040             if (n == 2)
1041                 d = a(0, 0) * a(1, 1) - a(1, 0) * a(0, 1);
1042             else
1043             {
1044                 d = 0;
1045                 for (uint j1 = 1; j1 <= n; j1++)
1046                 {
1047                     for (uint i = 2; i <= n; i++)
1048                     {
1049                         int j2 = 1;
1050                         for (uint j = 1; j <= n; j++)
1051                         {
1052                             if (j != j1)
1053                             {
1054                                 m(i - 2, j2 - 1) = a(i - 1, j - 1);
1055                                 j2++;
1056                             }
1057                         }
1058                     }
1059                     d += (((1 + j1) & 1) ? -1.0f : 1.0f) * a(1 - 1, j1 - 1) * det_helper(m, n - 1);
1060                 }
1061             }
1062             return d;
1063         }
1064     };
1065
1066     typedef matrix<2, 2, float> matrix22F;
1067     typedef matrix<2, 2, double> matrix22D;
1068
1069     typedef matrix<3, 3, float> matrix33F;
1070     typedef matrix<3, 3, double> matrix33D;
1071
1072     typedef matrix<4, 4, float> matrix44F;
1073     typedef matrix<4, 4, double> matrix44D;
1074
1075     typedef matrix<8, 8, float> matrix88F;
1076
1077     // These helpers create good old D3D-style matrices.
1078     inline matrix44F matrix44F_make_perspective_offcenter_lh(float l, float r, float b, float t, float nz, float fz)
1079     {
1080         float two_nz = 2.0f * nz;
1081         float one_over_width = 1.0f / (r - l);
1082         float one_over_height = 1.0f / (t - b);
1083
1084         matrix44F view_to_proj;
1085         view_to_proj[0].set(two_nz * one_over_width, 0.0f, 0.0f, 0.0f);
1086         view_to_proj[1].set(0.0f, two_nz * one_over_height, 0.0f, 0.0f);
1087         view_to_proj[2].set(-(l + r) * one_over_width, -(t + b) * one_over_height, fz / (fz - nz), 1.0f);
1088         view_to_proj[3].set(0.0f, 0.0f, -view_to_proj[2][2] * nz, 0.0f);
1089         return view_to_proj;
1090     }
1091
1092     // fov_y: full Y field of view (radians)
1093     // aspect: viewspace width/height
1094     inline matrix44F matrix44F_make_perspective_fov_lh(float fov_y, float aspect, float nz, float fz)
1095     {
1096         double sin_fov = sin(0.5f * fov_y);
1097         double cos_fov = cos(0.5f * fov_y);
1098
1099         float y_scale = static_cast<float>(cos_fov / sin_fov);
1100         float x_scale = static_cast<float>(y_scale / aspect);
1101
1102         matrix44F view_to_proj;
1103         view_to_proj[0].set(x_scale, 0, 0, 0);
1104         view_to_proj[1].set(0, y_scale, 0, 0);
1105         view_to_proj[2].set(0, 0, fz / (fz - nz), 1);
1106         view_to_proj[3].set(0, 0, -nz * fz / (fz - nz), 0);
1107         return view_to_proj;
1108     }
1109
1110     inline matrix44F matrix44F_make_ortho_offcenter_lh(float l, float r, float b, float t, float nz, float fz)
1111     {
1112         matrix44F view_to_proj;
1113         view_to_proj[0].set(2.0f / (r - l), 0.0f, 0.0f, 0.0f);
1114         view_to_proj[1].set(0.0f, 2.0f / (t - b), 0.0f, 0.0f);
1115         view_to_proj[2].set(0.0f, 0.0f, 1.0f / (fz - nz), 0.0f);
1116         view_to_proj[3].set((l + r) / (l - r), (t + b) / (b - t), nz / (nz - fz), 1.0f);
1117         return view_to_proj;
1118     }
1119
1120     inline matrix44F matrix44F_make_ortho_lh(float w, float h, float nz, float fz)
1121     {
1122         return matrix44F_make_ortho_offcenter_lh(-w * .5f, w * .5f, -h * .5f, h * .5f, nz, fz);
1123     }
1124
1125     inline matrix44F matrix44F_make_projection_to_screen_d3d(int x, int y, int w, int h, float min_z, float max_z)
1126     {
1127         matrix44F proj_to_screen;
1128         proj_to_screen[0].set(w * .5f, 0.0f, 0.0f, 0.0f);
1129         proj_to_screen[1].set(0, h * -.5f, 0.0f, 0.0f);
1130         proj_to_screen[2].set(0, 0.0f, max_z - min_z, 0.0f);
1131         proj_to_screen[3].set(x + w * .5f, y + h * .5f, min_z, 1.0f);
1132         return proj_to_screen;
1133     }
1134
1135     inline matrix44F matrix44F_make_lookat_lh(const vec3F &camera_pos, const vec3F &look_at, const vec3F &camera_up, float camera_roll_ang_in_radians)
1136     {
1137         vec4F col2(look_at - camera_pos);
1138         VOGL_ASSERT(col2.is_vector());
1139         if (col2.normalize() == 0.0f)
1140             col2.set(0, 0, 1, 0);
1141
1142         vec4F col1(camera_up);
1143         VOGL_ASSERT(col1.is_vector());
1144         if (!col2[0] && !col2[2])
1145             col1.set(-1.0f, 0.0f, 0.0f, 0.0f);
1146
1147         if ((col1.dot(col2)) > .9999f)
1148             col1.set(0.0f, 1.0f, 0.0f, 0.0f);
1149
1150         vec4F col0(vec4F::cross3(col1, col2).normalize_in_place());
1151         col1 = vec4F::cross3(col2, col0).normalize_in_place();
1152
1153         matrix44F rotm(matrix44F::make_identity_matrix());
1154         rotm.set_col(0, col0);
1155         rotm.set_col(1, col1);
1156         rotm.set_col(2, col2);
1157         return matrix44F::make_translate_matrix(-camera_pos[0], -camera_pos[1], -camera_pos[2]) * rotm * matrix44F::make_rotate_matrix(2, camera_roll_ang_in_radians);
1158     }
1159
1160 } // namespace vogl