git.cworth.org Git - vogl/blob - 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