libs/math/matrix.h

   1 /*
   2    Copyright (C) 2001-2006, William Joseph.
   3    All Rights Reserved.
   4
   5    This file is part of GtkRadiant.
   6
   7    GtkRadiant is free software; you can redistribute it and/or modify
   8    it under the terms of the GNU General Public License as published by
   9    the Free Software Foundation; either version 2 of the License, or
  10    (at your option) any later version.
  11
  12    GtkRadiant is distributed in the hope that it will be useful,
  13    but WITHOUT ANY WARRANTY; without even the implied warranty of
  14    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15    GNU General Public License for more details.
  16
  17    You should have received a copy of the GNU General Public License
  18    along with GtkRadiant; if not, write to the Free Software
  19    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20  */
  21
  22 #if !defined( INCLUDED_MATH_MATRIX_H )
  23 #define INCLUDED_MATH_MATRIX_H
  24
  25 /// \file
  26 /// \brief Matrix data types and related operations.
  27
  28 #include "math/vector.h"
  29
  30 /// \brief A 4x4 matrix stored in single-precision floating-point.
  31 class Matrix4
  32 {
  33 float m_elements[16];
  34 public:
  35
  36 Matrix4(){
  37 }
  38 Matrix4( float xx_, float xy_, float xz_, float xw_,
  39                  float yx_, float yy_, float yz_, float yw_,
  40                  float zx_, float zy_, float zz_, float zw_,
  41                  float tx_, float ty_, float tz_, float tw_ ){
  42         xx() = xx_;
  43         xy() = xy_;
  44         xz() = xz_;
  45         xw() = xw_;
  46         yx() = yx_;
  47         yy() = yy_;
  48         yz() = yz_;
  49         yw() = yw_;
  50         zx() = zx_;
  51         zy() = zy_;
  52         zz() = zz_;
  53         zw() = zw_;
  54         tx() = tx_;
  55         ty() = ty_;
  56         tz() = tz_;
  57         tw() = tw_;
  58 }
  59
  60 float& xx(){
  61         return m_elements[0];
  62 }
  63 const float& xx() const {
  64         return m_elements[0];
  65 }
  66 float& xy(){
  67         return m_elements[1];
  68 }
  69 const float& xy() const {
  70         return m_elements[1];
  71 }
  72 float& xz(){
  73         return m_elements[2];
  74 }
  75 const float& xz() const {
  76         return m_elements[2];
  77 }
  78 float& xw(){
  79         return m_elements[3];
  80 }
  81 const float& xw() const {
  82         return m_elements[3];
  83 }
  84 float& yx(){
  85         return m_elements[4];
  86 }
  87 const float& yx() const {
  88         return m_elements[4];
  89 }
  90 float& yy(){
  91         return m_elements[5];
  92 }
  93 const float& yy() const {
  94         return m_elements[5];
  95 }
  96 float& yz(){
  97         return m_elements[6];
  98 }
  99 const float& yz() const {
 100         return m_elements[6];
 101 }
 102 float& yw(){
 103         return m_elements[7];
 104 }
 105 const float& yw() const {
 106         return m_elements[7];
 107 }
 108 float& zx(){
 109         return m_elements[8];
 110 }
 111 const float& zx() const {
 112         return m_elements[8];
 113 }
 114 float& zy(){
 115         return m_elements[9];
 116 }
 117 const float& zy() const {
 118         return m_elements[9];
 119 }
 120 float& zz(){
 121         return m_elements[10];
 122 }
 123 const float& zz() const {
 124         return m_elements[10];
 125 }
 126 float& zw(){
 127         return m_elements[11];
 128 }
 129 const float& zw() const {
 130         return m_elements[11];
 131 }
 132 float& tx(){
 133         return m_elements[12];
 134 }
 135 const float& tx() const {
 136         return m_elements[12];
 137 }
 138 float& ty(){
 139         return m_elements[13];
 140 }
 141 const float& ty() const {
 142         return m_elements[13];
 143 }
 144 float& tz(){
 145         return m_elements[14];
 146 }
 147 const float& tz() const {
 148         return m_elements[14];
 149 }
 150 float& tw(){
 151         return m_elements[15];
 152 }
 153 const float& tw() const {
 154         return m_elements[15];
 155 }
 156
 157 Vector4& x(){
 158         return reinterpret_cast<Vector4&>( xx() );
 159 }
 160 const Vector4& x() const {
 161         return reinterpret_cast<const Vector4&>( xx() );
 162 }
 163 Vector4& y(){
 164         return reinterpret_cast<Vector4&>( yx() );
 165 }
 166 const Vector4& y() const {
 167         return reinterpret_cast<const Vector4&>( yx() );
 168 }
 169 Vector4& z(){
 170         return reinterpret_cast<Vector4&>( zx() );
 171 }
 172 const Vector4& z() const {
 173         return reinterpret_cast<const Vector4&>( zx() );
 174 }
 175 Vector4& t(){
 176         return reinterpret_cast<Vector4&>( tx() );
 177 }
 178 const Vector4& t() const {
 179         return reinterpret_cast<const Vector4&>( tx() );
 180 }
 181
 182 const float& index( std::size_t i ) const {
 183         return m_elements[i];
 184 }
 185 float& index( std::size_t i ){
 186         return m_elements[i];
 187 }
 188 const float& operator[]( std::size_t i ) const {
 189         return m_elements[i];
 190 }
 191 float& operator[]( std::size_t i ){
 192         return m_elements[i];
 193 }
 194 const float& index( std::size_t r, std::size_t c ) const {
 195         return m_elements[( r << 2 ) + c];
 196 }
 197 float& index( std::size_t r, std::size_t c ){
 198         return m_elements[( r << 2 ) + c];
 199 }
 200 };
 201
 202 /// \brief The 4x4 identity matrix.
 203 const Matrix4 g_matrix4_identity(
 204         1, 0, 0, 0,
 205         0, 1, 0, 0,
 206         0, 0, 1, 0,
 207         0, 0, 0, 1
 208         );
 209
 210
 211 /// \brief Returns true if \p self and \p other are exactly element-wise equal.
 212 inline bool operator==( const Matrix4& self, const Matrix4& other ){
 213         return self.xx() == other.xx() && self.xy() == other.xy() && self.xz() == other.xz() && self.xw() == other.xw()
 214                    && self.yx() == other.yx() && self.yy() == other.yy() && self.yz() == other.yz() && self.yw() == other.yw()
 215                    && self.zx() == other.zx() && self.zy() == other.zy() && self.zz() == other.zz() && self.zw() == other.zw()
 216                    && self.tx() == other.tx() && self.ty() == other.ty() && self.tz() == other.tz() && self.tw() == other.tw();
 217 }
 218
 219 /// \brief Returns true if \p self and \p other are exactly element-wise equal.
 220 inline bool matrix4_equal( const Matrix4& self, const Matrix4& other ){
 221         return self == other;
 222 }
 223
 224 /// \brief Returns true if \p self and \p other are element-wise equal within \p epsilon.
 225 inline bool matrix4_equal_epsilon( const Matrix4& self, const Matrix4& other, float epsilon ){
 226         return float_equal_epsilon( self.xx(), other.xx(), epsilon )
 227                    && float_equal_epsilon( self.xy(), other.xy(), epsilon )
 228                    && float_equal_epsilon( self.xz(), other.xz(), epsilon )
 229                    && float_equal_epsilon( self.xw(), other.xw(), epsilon )
 230                    && float_equal_epsilon( self.yx(), other.yx(), epsilon )
 231                    && float_equal_epsilon( self.yy(), other.yy(), epsilon )
 232                    && float_equal_epsilon( self.yz(), other.yz(), epsilon )
 233                    && float_equal_epsilon( self.yw(), other.yw(), epsilon )
 234                    && float_equal_epsilon( self.zx(), other.zx(), epsilon )
 235                    && float_equal_epsilon( self.zy(), other.zy(), epsilon )
 236                    && float_equal_epsilon( self.zz(), other.zz(), epsilon )
 237                    && float_equal_epsilon( self.zw(), other.zw(), epsilon )
 238                    && float_equal_epsilon( self.tx(), other.tx(), epsilon )
 239                    && float_equal_epsilon( self.ty(), other.ty(), epsilon )
 240                    && float_equal_epsilon( self.tz(), other.tz(), epsilon )
 241                    && float_equal_epsilon( self.tw(), other.tw(), epsilon );
 242 }
 243
 244 /// \brief Returns true if \p self and \p other are exactly element-wise equal.
 245 /// \p self and \p other must be affine.
 246 inline bool matrix4_affine_equal( const Matrix4& self, const Matrix4& other ){
 247         return self[0] == other[0]
 248                    && self[1] == other[1]
 249                    && self[2] == other[2]
 250                    && self[4] == other[4]
 251                    && self[5] == other[5]
 252                    && self[6] == other[6]
 253                    && self[8] == other[8]
 254                    && self[9] == other[9]
 255                    && self[10] == other[10]
 256                    && self[12] == other[12]
 257                    && self[13] == other[13]
 258                    && self[14] == other[14];
 259 }
 260
 261 enum Matrix4Handedness
 262 {
 263         MATRIX4_RIGHTHANDED = 0,
 264         MATRIX4_LEFTHANDED = 1,
 265 };
 266
 267 /// \brief Returns MATRIX4_RIGHTHANDED if \p self is right-handed, else returns MATRIX4_LEFTHANDED.
 268 inline Matrix4Handedness matrix4_handedness( const Matrix4& self ){
 269         return (
 270                            vector3_dot(
 271                                    vector3_cross( vector4_to_vector3( self.x() ), vector4_to_vector3( self.y() ) ),
 272                                    vector4_to_vector3( self.z() )
 273                                    )
 274                            < 0.0
 275                            ) ? MATRIX4_LEFTHANDED : MATRIX4_RIGHTHANDED;
 276 }
 277
 278
 279
 280
 281
 282 /// \brief Returns \p self post-multiplied by \p other.
 283 inline Matrix4 matrix4_multiplied_by_matrix4( const Matrix4& self, const Matrix4& other ){
 284         return Matrix4(
 285                            other[0] * self[0] + other[1] * self[4] + other[2] * self[8] + other[3] * self[12],
 286                            other[0] * self[1] + other[1] * self[5] + other[2] * self[9] + other[3] * self[13],
 287                            other[0] * self[2] + other[1] * self[6] + other[2] * self[10] + other[3] * self[14],
 288                            other[0] * self[3] + other[1] * self[7] + other[2] * self[11] + other[3] * self[15],
 289                            other[4] * self[0] + other[5] * self[4] + other[6] * self[8] + other[7] * self[12],
 290                            other[4] * self[1] + other[5] * self[5] + other[6] * self[9] + other[7] * self[13],
 291                            other[4] * self[2] + other[5] * self[6] + other[6] * self[10] + other[7] * self[14],
 292                            other[4] * self[3] + other[5] * self[7] + other[6] * self[11] + other[7] * self[15],
 293                            other[8] * self[0] + other[9] * self[4] + other[10] * self[8] + other[11] * self[12],
 294                            other[8] * self[1] + other[9] * self[5] + other[10] * self[9] + other[11] * self[13],
 295                            other[8] * self[2] + other[9] * self[6] + other[10] * self[10] + other[11] * self[14],
 296                            other[8] * self[3] + other[9] * self[7] + other[10] * self[11] + other[11] * self[15],
 297                            other[12] * self[0] + other[13] * self[4] + other[14] * self[8] + other[15] * self[12],
 298                            other[12] * self[1] + other[13] * self[5] + other[14] * self[9] + other[15] * self[13],
 299                            other[12] * self[2] + other[13] * self[6] + other[14] * self[10] + other[15] * self[14],
 300                            other[12] * self[3] + other[13] * self[7] + other[14] * self[11] + other[15] * self[15]
 301                            );
 302 }
 303
 304 /// \brief Post-multiplies \p self by \p other in-place.
 305 inline void matrix4_multiply_by_matrix4( Matrix4& self, const Matrix4& other ){
 306         self = matrix4_multiplied_by_matrix4( self, other );
 307 }
 308
 309
 310 /// \brief Returns \p self pre-multiplied by \p other.
 311 inline Matrix4 matrix4_premultiplied_by_matrix4( const Matrix4& self, const Matrix4& other ){
 312 #if 1
 313         return matrix4_multiplied_by_matrix4( other, self );
 314 #else
 315         return Matrix4(
 316                            self[0] * other[0] + self[1] * other[4] + self[2] * other[8] + self[3] * other[12],
 317                            self[0] * other[1] + self[1] * other[5] + self[2] * other[9] + self[3] * other[13],
 318                            self[0] * other[2] + self[1] * other[6] + self[2] * other[10] + self[3] * other[14],
 319                            self[0] * other[3] + self[1] * other[7] + self[2] * other[11] + self[3] * other[15],
 320                            self[4] * other[0] + self[5] * other[4] + self[6] * other[8] + self[7] * other[12],
 321                            self[4] * other[1] + self[5] * other[5] + self[6] * other[9] + self[7] * other[13],
 322                            self[4] * other[2] + self[5] * other[6] + self[6] * other[10] + self[7] * other[14],
 323                            self[4] * other[3] + self[5] * other[7] + self[6] * other[11] + self[7] * other[15],
 324                            self[8] * other[0] + self[9] * other[4] + self[10] * other[8] + self[11] * other[12],
 325                            self[8] * other[1] + self[9] * other[5] + self[10] * other[9] + self[11] * other[13],
 326                            self[8] * other[2] + self[9] * other[6] + self[10] * other[10] + self[11] * other[14],
 327                            self[8] * other[3] + self[9] * other[7] + self[10] * other[11] + self[11] * other[15],
 328                            self[12] * other[0] + self[13] * other[4] + self[14] * other[8] + self[15] * other[12],
 329                            self[12] * other[1] + self[13] * other[5] + self[14] * other[9] + self[15] * other[13],
 330                            self[12] * other[2] + self[13] * other[6] + self[14] * other[10] + self[15] * other[14],
 331                            self[12] * other[3] + self[13] * other[7] + self[14] * other[11] + self[15] * other[15]
 332                            );
 333 #endif
 334 }
 335
 336 /// \brief Pre-multiplies \p self by \p other in-place.
 337 inline void matrix4_premultiply_by_matrix4( Matrix4& self, const Matrix4& other ){
 338         self = matrix4_premultiplied_by_matrix4( self, other );
 339 }
 340
 341 /// \brief returns true if \p transform is affine.
 342 inline bool matrix4_is_affine( const Matrix4& transform ){
 343         return transform[3] == 0 && transform[7] == 0 && transform[11] == 0 && transform[15] == 1;
 344 }
 345
 346 /// \brief Returns \p self post-multiplied by \p other.
 347 /// \p self and \p other must be affine.
 348 inline Matrix4 matrix4_affine_multiplied_by_matrix4( const Matrix4& self, const Matrix4& other ){
 349         return Matrix4(
 350                            other[0] * self[0] + other[1] * self[4] + other[2] * self[8],
 351                            other[0] * self[1] + other[1] * self[5] + other[2] * self[9],
 352                            other[0] * self[2] + other[1] * self[6] + other[2] * self[10],
 353                            0,
 354                            other[4] * self[0] + other[5] * self[4] + other[6] * self[8],
 355                            other[4] * self[1] + other[5] * self[5] + other[6] * self[9],
 356                            other[4] * self[2] + other[5] * self[6] + other[6] * self[10],
 357                            0,
 358                            other[8] * self[0] + other[9] * self[4] + other[10] * self[8],
 359                            other[8] * self[1] + other[9] * self[5] + other[10] * self[9],
 360                            other[8] * self[2] + other[9] * self[6] + other[10] * self[10],
 361                            0,
 362                            other[12] * self[0] + other[13] * self[4] + other[14] * self[8] + self[12],
 363                            other[12] * self[1] + other[13] * self[5] + other[14] * self[9] + self[13],
 364                            other[12] * self[2] + other[13] * self[6] + other[14] * self[10] + self[14],
 365                            1
 366                            );
 367 }
 368
 369 /// \brief Post-multiplies \p self by \p other in-place.
 370 /// \p self and \p other must be affine.
 371 inline void matrix4_affine_multiply_by_matrix4( Matrix4& self, const Matrix4& other ){
 372         self = matrix4_affine_multiplied_by_matrix4( self, other );
 373 }
 374
 375 /// \brief Returns \p self pre-multiplied by \p other.
 376 /// \p self and \p other must be affine.
 377 inline Matrix4 matrix4_affine_premultiplied_by_matrix4( const Matrix4& self, const Matrix4& other ){
 378 #if 1
 379         return matrix4_affine_multiplied_by_matrix4( other, self );
 380 #else
 381         return Matrix4(
 382                            self[0] * other[0] + self[1] * other[4] + self[2] * other[8],
 383                            self[0] * other[1] + self[1] * other[5] + self[2] * other[9],
 384                            self[0] * other[2] + self[1] * other[6] + self[2] * other[10],
 385                            0,
 386                            self[4] * other[0] + self[5] * other[4] + self[6] * other[8],
 387                            self[4] * other[1] + self[5] * other[5] + self[6] * other[9],
 388                            self[4] * other[2] + self[5] * other[6] + self[6] * other[10],
 389                            0,
 390                            self[8] * other[0] + self[9] * other[4] + self[10] * other[8],
 391                            self[8] * other[1] + self[9] * other[5] + self[10] * other[9],
 392                            self[8] * other[2] + self[9] * other[6] + self[10] * other[10],
 393                            0,
 394                            self[12] * other[0] + self[13] * other[4] + self[14] * other[8] + other[12],
 395                            self[12] * other[1] + self[13] * other[5] + self[14] * other[9] + other[13],
 396                            self[12] * other[2] + self[13] * other[6] + self[14] * other[10] + other[14],
 397                            1
 398                            )
 399                    );
 400 #endif
 401 }
 402
 403 /// \brief Pre-multiplies \p self by \p other in-place.
 404 /// \p self and \p other must be affine.
 405 inline void matrix4_affine_premultiply_by_matrix4( Matrix4& self, const Matrix4& other ){
 406         self = matrix4_affine_premultiplied_by_matrix4( self, other );
 407 }
 408
 409 /// \brief Returns \p point transformed by \p self.
 410 template<typename Element>
 411 inline BasicVector3<Element> matrix4_transformed_point( const Matrix4& self, const BasicVector3<Element>& point ){
 412         return BasicVector3<Element>(
 413                            static_cast<Element>( self[0]  * point[0] + self[4]  * point[1] + self[8]  * point[2] + self[12] ),
 414                            static_cast<Element>( self[1]  * point[0] + self[5]  * point[1] + self[9]  * point[2] + self[13] ),
 415                            static_cast<Element>( self[2]  * point[0] + self[6]  * point[1] + self[10] * point[2] + self[14] )
 416                            );
 417 }
 418
 419 /// \brief Transforms \p point by \p self in-place.
 420 template<typename Element>
 421 inline void matrix4_transform_point( const Matrix4& self, BasicVector3<Element>& point ){
 422         point = matrix4_transformed_point( self, point );
 423 }
 424
 425 /// \brief Returns \p direction transformed by \p self.
 426 template<typename Element>
 427 inline BasicVector3<Element> matrix4_transformed_direction( const Matrix4& self, const BasicVector3<Element>& direction ){
 428         return BasicVector3<Element>(
 429                            static_cast<Element>( self[0]  * direction[0] + self[4]  * direction[1] + self[8]  * direction[2] ),
 430                            static_cast<Element>( self[1]  * direction[0] + self[5]  * direction[1] + self[9]  * direction[2] ),
 431                            static_cast<Element>( self[2]  * direction[0] + self[6]  * direction[1] + self[10] * direction[2] )
 432                            );
 433 }
 434
 435 /// \brief Transforms \p direction by \p self in-place.
 436 template<typename Element>
 437 inline void matrix4_transform_direction( const Matrix4& self, BasicVector3<Element>& normal ){
 438         normal = matrix4_transformed_direction( self, normal );
 439 }
 440
 441 /// \brief Returns \p vector4 transformed by \p self.
 442 inline Vector4 matrix4_transformed_vector4( const Matrix4& self, const Vector4& vector4 ){
 443         return Vector4(
 444                            self[0]  * vector4[0] + self[4]  * vector4[1] + self[8]  * vector4[2] + self[12] * vector4[3],
 445                            self[1]  * vector4[0] + self[5]  * vector4[1] + self[9]  * vector4[2] + self[13] * vector4[3],
 446                            self[2]  * vector4[0] + self[6]  * vector4[1] + self[10] * vector4[2] + self[14] * vector4[3],
 447                            self[3]  * vector4[0] + self[7]  * vector4[1] + self[11] * vector4[2] + self[15] * vector4[3]
 448                            );
 449 }
 450
 451 /// \brief Transforms \p vector4 by \p self in-place.
 452 inline void matrix4_transform_vector4( const Matrix4& self, Vector4& vector4 ){
 453         vector4 = matrix4_transformed_vector4( self, vector4 );
 454 }
 455
 456
 457 /// \brief Transposes \p self in-place.
 458 inline void matrix4_transpose( Matrix4& self ){
 459         std::swap( self.xy(), self.yx() );
 460         std::swap( self.xz(), self.zx() );
 461         std::swap( self.xw(), self.tx() );
 462         std::swap( self.yz(), self.zy() );
 463         std::swap( self.yw(), self.ty() );
 464         std::swap( self.zw(), self.tz() );
 465 }
 466
 467 /// \brief Returns \p self transposed.
 468 inline Matrix4 matrix4_transposed( const Matrix4& self ){
 469         return Matrix4(
 470                            self.xx(),
 471                            self.yx(),
 472                            self.zx(),
 473                            self.tx(),
 474                            self.xy(),
 475                            self.yy(),
 476                            self.zy(),
 477                            self.ty(),
 478                            self.xz(),
 479                            self.yz(),
 480                            self.zz(),
 481                            self.tz(),
 482                            self.xw(),
 483                            self.yw(),
 484                            self.zw(),
 485                            self.tw()
 486                            );
 487 }
 488
 489
 490 /// \brief Inverts an affine transform in-place.
 491 /// Adapted from Graphics Gems 2.
 492 inline Matrix4 matrix4_affine_inverse( const Matrix4& self ){
 493         Matrix4 result;
 494
 495         // determinant of rotation submatrix
 496         double det
 497                 = self[0] * ( self[5] * self[10] - self[9] * self[6] )
 498                   - self[1] * ( self[4] * self[10] - self[8] * self[6] )
 499                   + self[2] * ( self[4] * self[9] - self[8] * self[5] );
 500
 501         // throw exception here if (det*det < 1e-25)
 502
 503         // invert rotation submatrix
 504         det = 1.0 / det;
 505
 506         result[0] = static_cast<float>(  ( self[5] * self[10] - self[6] * self[9] ) * det );
 507         result[1] = static_cast<float>( -( self[1] * self[10] - self[2] * self[9] ) * det );
 508         result[2] = static_cast<float>(  ( self[1] * self[6] - self[2] * self[5] ) * det );
 509         result[3] = 0;
 510         result[4] = static_cast<float>( -( self[4] * self[10] - self[6] * self[8] ) * det );
 511         result[5] = static_cast<float>(  ( self[0] * self[10] - self[2] * self[8] ) * det );
 512         result[6] = static_cast<float>( -( self[0] * self[6] - self[2] * self[4] ) * det );
 513         result[7] = 0;
 514         result[8] = static_cast<float>(  ( self[4] * self[9] - self[5] * self[8] ) * det );
 515         result[9] = static_cast<float>( -( self[0] * self[9] - self[1] * self[8] ) * det );
 516         result[10] = static_cast<float>(  ( self[0] * self[5] - self[1] * self[4] ) * det );
 517         result[11] = 0;
 518
 519         // multiply translation part by rotation
 520         result[12] = -( self[12] * result[0] +
 521                                         self[13] * result[4] +
 522                                         self[14] * result[8] );
 523         result[13] = -( self[12] * result[1] +
 524                                         self[13] * result[5] +
 525                                         self[14] * result[9] );
 526         result[14] = -( self[12] * result[2] +
 527                                         self[13] * result[6] +
 528                                         self[14] * result[10] );
 529         result[15] = 1;
 530
 531         return result;
 532 }
 533
 534 inline void matrix4_affine_invert( Matrix4& self ){
 535         self = matrix4_affine_inverse( self );
 536 }
 537
 538 /// \brief A compile-time-constant integer.
 539 template<int VALUE_>
 540 struct IntegralConstant
 541 {
 542         enum unnamed_ { VALUE = VALUE_ };
 543 };
 544
 545 /// \brief A compile-time-constant row/column index into a 4x4 matrix.
 546 template<typename Row, typename Col>
 547 class Matrix4Index
 548 {
 549 public:
 550 typedef IntegralConstant<Row::VALUE> r;
 551 typedef IntegralConstant<Col::VALUE> c;
 552 typedef IntegralConstant<( r::VALUE * 4 ) + c::VALUE> i;
 553 };
 554
 555 /// \brief A functor which returns the cofactor of a 3x3 submatrix obtained by ignoring a given row and column of a 4x4 matrix.
 556 /// \param Row Defines the compile-time-constant integers x, y and z with values corresponding to the indices of the three rows to use.
 557 /// \param Col Defines the compile-time-constant integers x, y and z with values corresponding to the indices of the three columns to use.
 558 template<typename Row, typename Col>
 559 class Matrix4Cofactor
 560 {
 561 public:
 562 typedef typename Matrix4Index<typename Row::x, typename Col::x>::i xx;
 563 typedef typename Matrix4Index<typename Row::x, typename Col::y>::i xy;
 564 typedef typename Matrix4Index<typename Row::x, typename Col::z>::i xz;
 565 typedef typename Matrix4Index<typename Row::y, typename Col::x>::i yx;
 566 typedef typename Matrix4Index<typename Row::y, typename Col::y>::i yy;
 567 typedef typename Matrix4Index<typename Row::y, typename Col::z>::i yz;
 568 typedef typename Matrix4Index<typename Row::z, typename Col::x>::i zx;
 569 typedef typename Matrix4Index<typename Row::z, typename Col::y>::i zy;
 570 typedef typename Matrix4Index<typename Row::z, typename Col::z>::i zz;
 571 static double apply( const Matrix4& self ){
 572         return self[xx::VALUE] * ( self[yy::VALUE] * self[zz::VALUE] - self[zy::VALUE] * self[yz::VALUE] )
 573                    - self[xy::VALUE] * ( self[yx::VALUE] * self[zz::VALUE] - self[zx::VALUE] * self[yz::VALUE] )
 574                    + self[xz::VALUE] * ( self[yx::VALUE] * self[zy::VALUE] - self[zx::VALUE] * self[yy::VALUE] );
 575 }
 576 };
 577
 578 /// \brief The cofactor element indices for a 4x4 matrix row or column.
 579 /// \param Element The index of the element to ignore.
 580 template<int Element>
 581 class Cofactor4
 582 {
 583 public:
 584 typedef IntegralConstant<( Element <= 0 ) ? 1 : 0> x;
 585 typedef IntegralConstant<( Element <= 1 ) ? 2 : 1> y;
 586 typedef IntegralConstant<( Element <= 2 ) ? 3 : 2> z;
 587 };
 588
 589 /// \brief Returns the determinant of \p self.
 590 inline double matrix4_determinant( const Matrix4& self ){
 591         return self.xx() * Matrix4Cofactor< Cofactor4<0>, Cofactor4<0> >::apply( self )
 592                    - self.xy() * Matrix4Cofactor< Cofactor4<0>, Cofactor4<1> >::apply( self )
 593                    + self.xz() * Matrix4Cofactor< Cofactor4<0>, Cofactor4<2> >::apply( self )
 594                    - self.xw() * Matrix4Cofactor< Cofactor4<0>, Cofactor4<3> >::apply( self );
 595 }
 596
 597 /// \brief Returns the inverse of \p self using the Adjoint method.
 598 /// \todo Throw an exception if the determinant is zero.
 599 inline Matrix4 matrix4_full_inverse( const Matrix4& self ){
 600         double determinant = 1.0 / matrix4_determinant( self );
 601
 602         return Matrix4(
 603                            static_cast<float>( Matrix4Cofactor< Cofactor4<0>, Cofactor4<0> >::apply( self ) * determinant ),
 604                            static_cast<float>( -Matrix4Cofactor< Cofactor4<1>, Cofactor4<0> >::apply( self ) * determinant ),
 605                            static_cast<float>( Matrix4Cofactor< Cofactor4<2>, Cofactor4<0> >::apply( self ) * determinant ),
 606                            static_cast<float>( -Matrix4Cofactor< Cofactor4<3>, Cofactor4<0> >::apply( self ) * determinant ),
 607                            static_cast<float>( -Matrix4Cofactor< Cofactor4<0>, Cofactor4<1> >::apply( self ) * determinant ),
 608                            static_cast<float>( Matrix4Cofactor< Cofactor4<1>, Cofactor4<1> >::apply( self ) * determinant ),
 609                            static_cast<float>( -Matrix4Cofactor< Cofactor4<2>, Cofactor4<1> >::apply( self ) * determinant ),
 610                            static_cast<float>( Matrix4Cofactor< Cofactor4<3>, Cofactor4<1> >::apply( self ) * determinant ),
 611                            static_cast<float>( Matrix4Cofactor< Cofactor4<0>, Cofactor4<2> >::apply( self ) * determinant ),
 612                            static_cast<float>( -Matrix4Cofactor< Cofactor4<1>, Cofactor4<2> >::apply( self ) * determinant ),
 613                            static_cast<float>( Matrix4Cofactor< Cofactor4<2>, Cofactor4<2> >::apply( self ) * determinant ),
 614                            static_cast<float>( -Matrix4Cofactor< Cofactor4<3>, Cofactor4<2> >::apply( self ) * determinant ),
 615                            static_cast<float>( -Matrix4Cofactor< Cofactor4<0>, Cofactor4<3> >::apply( self ) * determinant ),
 616                            static_cast<float>( Matrix4Cofactor< Cofactor4<1>, Cofactor4<3> >::apply( self ) * determinant ),
 617                            static_cast<float>( -Matrix4Cofactor< Cofactor4<2>, Cofactor4<3> >::apply( self ) * determinant ),
 618                            static_cast<float>( Matrix4Cofactor< Cofactor4<3>, Cofactor4<3> >::apply( self ) * determinant )
 619                            );
 620 }
 621
 622 /// \brief Inverts \p self in-place using the Adjoint method.
 623 inline void matrix4_full_invert( Matrix4& self ){
 624         self = matrix4_full_inverse( self );
 625 }
 626
 627
 628 /// \brief Constructs a pure-translation matrix from \p translation.
 629 inline Matrix4 matrix4_translation_for_vec3( const Vector3& translation ){
 630         return Matrix4(
 631                            1, 0, 0, 0,
 632                            0, 1, 0, 0,
 633                            0, 0, 1, 0,
 634                            translation[0], translation[1], translation[2], 1
 635                            );
 636 }
 637
 638 /// \brief Returns the translation part of \p self.
 639 inline Vector3 matrix4_get_translation_vec3( const Matrix4& self ){
 640         return vector4_to_vector3( self.t() );
 641 }
 642
 643 /// \brief Concatenates \p self with \p translation.
 644 /// The concatenated \p translation occurs before \p self.
 645 inline void matrix4_translate_by_vec3( Matrix4& self, const Vector3& translation ){
 646         matrix4_multiply_by_matrix4( self, matrix4_translation_for_vec3( translation ) );
 647 }
 648
 649 /// \brief Returns \p self Concatenated with \p translation.
 650 /// The concatenated translation occurs before \p self.
 651 inline Matrix4 matrix4_translated_by_vec3( const Matrix4& self, const Vector3& translation ){
 652         return matrix4_multiplied_by_matrix4( self, matrix4_translation_for_vec3( translation ) );
 653 }
 654
 655
 656 #include "math/pi.h"
 657
 658 /// \brief Returns \p angle modulated by the range [0, 360).
 659 /// \p angle must be in the range [-360, 360).
 660 inline float angle_modulate_degrees_range( float angle ){
 661         return static_cast<float>( float_mod_range( angle, 360.0 ) );
 662 }
 663
 664 /// \brief Returns \p euler angles converted from radians to degrees.
 665 inline Vector3 euler_radians_to_degrees( const Vector3& euler ){
 666         return Vector3(
 667                            static_cast<float>( radians_to_degrees( euler.x() ) ),
 668                            static_cast<float>( radians_to_degrees( euler.y() ) ),
 669                            static_cast<float>( radians_to_degrees( euler.z() ) )
 670                            );
 671 }
 672
 673 /// \brief Returns \p euler angles converted from degrees to radians.
 674 inline Vector3 euler_degrees_to_radians( const Vector3& euler ){
 675         return Vector3(
 676                            static_cast<float>( degrees_to_radians( euler.x() ) ),
 677                            static_cast<float>( degrees_to_radians( euler.y() ) ),
 678                            static_cast<float>( degrees_to_radians( euler.z() ) )
 679                            );
 680 }
 681
 682
 683
 684 /// \brief Constructs a pure-rotation matrix about the x axis from sin \p s and cosine \p c of an angle.
 685 inline Matrix4 matrix4_rotation_for_sincos_x( float s, float c ){
 686         return Matrix4(
 687                            1, 0, 0, 0,
 688                            0, c, s, 0,
 689                            0,-s, c, 0,
 690                            0, 0, 0, 1
 691                            );
 692 }
 693
 694 /// \brief Constructs a pure-rotation matrix about the x axis from an angle in radians.
 695 inline Matrix4 matrix4_rotation_for_x( double x ){
 696         return matrix4_rotation_for_sincos_x( static_cast<float>( sin( x ) ), static_cast<float>( cos( x ) ) );
 697 }
 698
 699 /// \brief Constructs a pure-rotation matrix about the x axis from an angle in degrees.
 700 inline Matrix4 matrix4_rotation_for_x_degrees( float x ){
 701         return matrix4_rotation_for_x( degrees_to_radians( x ) );
 702 }
 703
 704 /// \brief Constructs a pure-rotation matrix about the y axis from sin \p s and cosine \p c of an angle.
 705 inline Matrix4 matrix4_rotation_for_sincos_y( float s, float c ){
 706         return Matrix4(
 707                            c, 0,-s, 0,
 708                            0, 1, 0, 0,
 709                            s, 0, c, 0,
 710                            0, 0, 0, 1
 711                            );
 712 }
 713
 714 /// \brief Constructs a pure-rotation matrix about the y axis from an angle in radians.
 715 inline Matrix4 matrix4_rotation_for_y( double y ){
 716         return matrix4_rotation_for_sincos_y( static_cast<float>( sin( y ) ), static_cast<float>( cos( y ) ) );
 717 }
 718
 719 /// \brief Constructs a pure-rotation matrix about the y axis from an angle in degrees.
 720 inline Matrix4 matrix4_rotation_for_y_degrees( float y ){
 721         return matrix4_rotation_for_y( degrees_to_radians( y ) );
 722 }
 723
 724 /// \brief Constructs a pure-rotation matrix about the z axis from sin \p s and cosine \p c of an angle.
 725 inline Matrix4 matrix4_rotation_for_sincos_z( float s, float c ){
 726         return Matrix4(
 727                            c, s, 0, 0,
 728                            -s, c, 0, 0,
 729                            0, 0, 1, 0,
 730                            0, 0, 0, 1
 731                            );
 732 }
 733
 734 /// \brief Constructs a pure-rotation matrix about the z axis from an angle in radians.
 735 inline Matrix4 matrix4_rotation_for_z( double z ){
 736         return matrix4_rotation_for_sincos_z( static_cast<float>( sin( z ) ), static_cast<float>( cos( z ) ) );
 737 }
 738
 739 /// \brief Constructs a pure-rotation matrix about the z axis from an angle in degrees.
 740 inline Matrix4 matrix4_rotation_for_z_degrees( float z ){
 741         return matrix4_rotation_for_z( degrees_to_radians( z ) );
 742 }
 743
 744 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (x, y, z).
 745 /*! \verbatim
 746    clockwise rotation around X, Y, Z, facing along axis
 747    1  0   0    cy 0 -sy   cz  sz 0
 748    0  cx  sx   0  1  0   -sz  cz 0
 749    0 -sx  cx   sy 0  cy   0   0  1
 750
 751    rows of Z by cols of Y
 752    cy*cz -sy*cz+sz -sy*sz+cz
 753    -sz*cy -sz*sy+cz
 754
 755    .. or something like that..
 756
 757    final rotation is Z * Y * X
 758    cy*cz -sx*-sy*cz+cx*sz  cx*-sy*sz+sx*cz
 759    -cy*sz  sx*sy*sz+cx*cz  -cx*-sy*sz+sx*cz
 760    sy    -sx*cy            cx*cy
 761
 762    transposed
 763    cy.cz + 0.sz + sy.0            cy.-sz + 0 .cz +  sy.0          cy.0  + 0 .0  +   sy.1       |
 764    sx.sy.cz + cx.sz + -sx.cy.0    sx.sy.-sz + cx.cz + -sx.cy.0    sx.sy.0  + cx.0  + -sx.cy.1  |
 765    -cx.sy.cz + sx.sz +  cx.cy.0   -cx.sy.-sz + sx.cz +  cx.cy.0   -cx.sy.0  + 0 .0  +  cx.cy.1  |
 766    \endverbatim */
 767 inline Matrix4 matrix4_rotation_for_euler_xyz( const Vector3& euler ){
 768 #if 1
 769
 770         double cx = cos( euler[0] );
 771         double sx = sin( euler[0] );
 772         double cy = cos( euler[1] );
 773         double sy = sin( euler[1] );
 774         double cz = cos( euler[2] );
 775         double sz = sin( euler[2] );
 776
 777         return Matrix4(
 778                            static_cast<float>( cy * cz ),
 779                            static_cast<float>( cy * sz ),
 780                            static_cast<float>( -sy ),
 781                            0,
 782                            static_cast<float>( sx * sy * cz + cx * -sz ),
 783                            static_cast<float>( sx * sy * sz + cx * cz ),
 784                            static_cast<float>( sx * cy ),
 785                            0,
 786                            static_cast<float>( cx * sy * cz + sx * sz ),
 787                            static_cast<float>( cx * sy * sz + -sx * cz ),
 788                            static_cast<float>( cx * cy ),
 789                            0,
 790                            0,
 791                            0,
 792                            0,
 793                            1
 794                            );
 795
 796 #else
 797
 798         return matrix4_premultiply_by_matrix4(
 799                            matrix4_premultiply_by_matrix4(
 800                                    matrix4_rotation_for_x( euler[0] ),
 801                                    matrix4_rotation_for_y( euler[1] )
 802                                    ),
 803                            matrix4_rotation_for_z( euler[2] )
 804                            );
 805
 806 #endif
 807 }
 808
 809 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degrees) in the order (x, y, z).
 810 inline Matrix4 matrix4_rotation_for_euler_xyz_degrees( const Vector3& euler ){
 811         return matrix4_rotation_for_euler_xyz( euler_degrees_to_radians( euler ) );
 812 }
 813
 814 /// \brief Concatenates \p self with the rotation transform produced by \p euler angles (degrees) in the order (x, y, z).
 815 /// The concatenated rotation occurs before \p self.
 816 inline void matrix4_rotate_by_euler_xyz_degrees( Matrix4& self, const Vector3& euler ){
 817         matrix4_multiply_by_matrix4( self, matrix4_rotation_for_euler_xyz_degrees( euler ) );
 818 }
 819
 820
 821 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (y, z, x).
 822 inline Matrix4 matrix4_rotation_for_euler_yzx( const Vector3& euler ){
 823         return matrix4_premultiplied_by_matrix4(
 824                            matrix4_premultiplied_by_matrix4(
 825                                    matrix4_rotation_for_y( euler[1] ),
 826                                    matrix4_rotation_for_z( euler[2] )
 827                                    ),
 828                            matrix4_rotation_for_x( euler[0] )
 829                            );
 830 }
 831
 832 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degrees) in the order (y, z, x).
 833 inline Matrix4 matrix4_rotation_for_euler_yzx_degrees( const Vector3& euler ){
 834         return matrix4_rotation_for_euler_yzx( euler_degrees_to_radians( euler ) );
 835 }
 836
 837 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (x, z, y).
 838 inline Matrix4 matrix4_rotation_for_euler_xzy( const Vector3& euler ){
 839         return matrix4_premultiplied_by_matrix4(
 840                            matrix4_premultiplied_by_matrix4(
 841                                    matrix4_rotation_for_x( euler[0] ),
 842                                    matrix4_rotation_for_z( euler[2] )
 843                                    ),
 844                            matrix4_rotation_for_y( euler[1] )
 845                            );
 846 }
 847
 848 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degrees) in the order (x, z, y).
 849 inline Matrix4 matrix4_rotation_for_euler_xzy_degrees( const Vector3& euler ){
 850         return matrix4_rotation_for_euler_xzy( euler_degrees_to_radians( euler ) );
 851 }
 852
 853 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (y, x, z).
 854 /*! \verbatim
 855  |  cy.cz + sx.sy.-sz + -cx.sy.0   0.cz + cx.-sz + sx.0   sy.cz + -sx.cy.-sz + cx.cy.0 |
 856  |  cy.sz + sx.sy.cz + -cx.sy.0    0.sz + cx.cz + sx.0    sy.sz + -sx.cy.cz + cx.cy.0  |
 857  |  cy.0 + sx.sy.0 + -cx.sy.1      0.0 + cx.0 + sx.1      sy.0 + -sx.cy.0 + cx.cy.1    |
 858    \endverbatim */
 859 inline Matrix4 matrix4_rotation_for_euler_yxz( const Vector3& euler ){
 860 #if 1
 861
 862         double cx = cos( euler[0] );
 863         double sx = sin( euler[0] );
 864         double cy = cos( euler[1] );
 865         double sy = sin( euler[1] );
 866         double cz = cos( euler[2] );
 867         double sz = sin( euler[2] );
 868
 869         return Matrix4(
 870                            static_cast<float>( cy * cz + sx * sy * -sz ),
 871                            static_cast<float>( cy * sz + sx * sy * cz ),
 872                            static_cast<float>( -cx * sy ),
 873                            0,
 874                            static_cast<float>( cx * -sz ),
 875                            static_cast<float>( cx * cz ),
 876                            static_cast<float>( sx ),
 877                            0,
 878                            static_cast<float>( sy * cz + -sx * cy * -sz ),
 879                            static_cast<float>( sy * sz + -sx * cy * cz ),
 880                            static_cast<float>( cx * cy ),
 881                            0,
 882                            0,
 883                            0,
 884                            0,
 885                            1
 886                            );
 887
 888 #else
 889
 890         return matrix4_premultiply_by_matrix4(
 891                            matrix4_premultiply_by_matrix4(
 892                                    matrix4_rotation_for_y( euler[1] ),
 893                                    matrix4_rotation_for_x( euler[0] )
 894                                    ),
 895                            matrix4_rotation_for_z( euler[2] )
 896                            );
 897
 898 #endif
 899 }
 900
 901 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degrees) in the order (y, x, z).
 902 inline Matrix4 matrix4_rotation_for_euler_yxz_degrees( const Vector3& euler ){
 903         return matrix4_rotation_for_euler_yxz( euler_degrees_to_radians( euler ) );
 904 }
 905
 906 /// \brief Returns \p self concatenated with the rotation transform produced by \p euler angles (degrees) in the order (y, x, z).
 907 /// The concatenated rotation occurs before \p self.
 908 inline Matrix4 matrix4_rotated_by_euler_yxz_degrees( const Matrix4& self, const Vector3& euler ){
 909         return matrix4_multiplied_by_matrix4( self, matrix4_rotation_for_euler_yxz_degrees( euler ) );
 910 }
 911
 912 /// \brief Concatenates \p self with the rotation transform produced by \p euler angles (degrees) in the order (y, x, z).
 913 /// The concatenated rotation occurs before \p self.
 914 inline void matrix4_rotate_by_euler_yxz_degrees( Matrix4& self, const Vector3& euler ){
 915         self = matrix4_rotated_by_euler_yxz_degrees( self, euler );
 916 }
 917
 918 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (z, x, y).
 919 inline Matrix4 matrix4_rotation_for_euler_zxy( const Vector3& euler ){
 920 #if 1
 921         return matrix4_premultiplied_by_matrix4(
 922                            matrix4_premultiplied_by_matrix4(
 923                                    matrix4_rotation_for_z( euler[2] ),
 924                                    matrix4_rotation_for_x( euler[0] )
 925                                    ),
 926                            matrix4_rotation_for_y( euler[1] )
 927                            );
 928 #else
 929         double cx = cos( euler[0] );
 930         double sx = sin( euler[0] );
 931         double cy = cos( euler[1] );
 932         double sy = sin( euler[1] );
 933         double cz = cos( euler[2] );
 934         double sz = sin( euler[2] );
 935
 936         return Matrix4(
 937                            static_cast<float>( cz * cy + sz * sx * sy ),
 938                            static_cast<float>( sz * cx ),
 939                            static_cast<float>( cz * -sy + sz * sx * cy ),
 940                            0,
 941                            static_cast<float>( -sz * cy + cz * sx * sy ),
 942                            static_cast<float>( cz * cx ),
 943                            static_cast<float>( -sz * -sy + cz * cx * cy ),
 944                            0,
 945                            static_cast<float>( cx * sy ),
 946                            static_cast<float>( -sx ),
 947                            static_cast<float>( cx * cy ),
 948                            0,
 949                            0,
 950                            0,
 951                            0,
 952                            1
 953                            );
 954 #endif
 955 }
 956
 957 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degres=es) in the order (z, x, y).
 958 inline Matrix4 matrix4_rotation_for_euler_zxy_degrees( const Vector3& euler ){
 959         return matrix4_rotation_for_euler_zxy( euler_degrees_to_radians( euler ) );
 960 }
 961
 962 /// \brief Returns \p self concatenated with the rotation transform produced by \p euler angles (degrees) in the order (z, x, y).
 963 /// The concatenated rotation occurs before \p self.
 964 inline Matrix4 matrix4_rotated_by_euler_zxy_degrees( const Matrix4& self, const Vector3& euler ){
 965         return matrix4_multiplied_by_matrix4( self, matrix4_rotation_for_euler_zxy_degrees( euler ) );
 966 }
 967
 968 /// \brief Concatenates \p self with the rotation transform produced by \p euler angles (degrees) in the order (z, x, y).
 969 /// The concatenated rotation occurs before \p self.
 970 inline void matrix4_rotate_by_euler_zxy_degrees( Matrix4& self, const Vector3& euler ){
 971         self = matrix4_rotated_by_euler_zxy_degrees( self, euler );
 972 }
 973
 974 /// \brief Constructs a pure-rotation matrix from a set of euler angles (radians) in the order (z, y, x).
 975 inline Matrix4 matrix4_rotation_for_euler_zyx( const Vector3& euler ){
 976 #if 1
 977
 978         double cx = cos( euler[0] );
 979         double sx = sin( euler[0] );
 980         double cy = cos( euler[1] );
 981         double sy = sin( euler[1] );
 982         double cz = cos( euler[2] );
 983         double sz = sin( euler[2] );
 984
 985         return Matrix4(
 986                            static_cast<float>( cy * cz ),
 987                            static_cast<float>( sx * sy * cz + cx * sz ),
 988                            static_cast<float>( cx * -sy * cz + sx * sz ),
 989                            0,
 990                            static_cast<float>( cy * -sz ),
 991                            static_cast<float>( sx * sy * -sz + cx * cz ),
 992                            static_cast<float>( cx * -sy * -sz + sx * cz ),
 993                            0,
 994                            static_cast<float>( sy ),
 995                            static_cast<float>( -sx * cy ),
 996                            static_cast<float>( cx * cy ),
 997                            0,
 998                            0,
 999                            0,
1000                            0,
1001                            1
1002                            );
1003
1004 #else
1005
1006         return matrix4_premultiply_by_matrix4(
1007                            matrix4_premultiply_by_matrix4(
1008                                    matrix4_rotation_for_z( euler[2] ),
1009                                    matrix4_rotation_for_y( euler[1] )
1010                                    ),
1011                            matrix4_rotation_for_x( euler[0] )
1012                            );
1013
1014 #endif
1015 }
1016
1017 /// \brief Constructs a pure-rotation matrix from a set of euler angles (degrees) in the order (z, y, x).
1018 inline Matrix4 matrix4_rotation_for_euler_zyx_degrees( const Vector3& euler ){
1019         return matrix4_rotation_for_euler_zyx( euler_degrees_to_radians( euler ) );
1020 }
1021
1022
1023 /// \brief Calculates and returns a set of euler angles that produce the rotation component of \p self when applied in the order (x, y, z).
1024 /// \p self must be affine and orthonormal (unscaled) to produce a meaningful result.
1025 inline Vector3 matrix4_get_rotation_euler_xyz( const Matrix4& self ){
1026         double a = asin( -self[2] );
1027         double ca = cos( a );
1028
1029         if ( fabs( ca ) > 0.005 ) { // Gimbal lock?
1030                 return Vector3(
1031                                    static_cast<float>( atan2( self[6] / ca, self[10] / ca ) ),
1032                                    static_cast<float>( a ),
1033                                    static_cast<float>( atan2( self[1] / ca, self[0] / ca ) )
1034                                    );
1035         }
1036         else // Gimbal lock has occurred
1037         {
1038                 return Vector3(
1039                                    static_cast<float>( atan2( -self[9], self[5] ) ),
1040                                    static_cast<float>( a ),
1041                                    0
1042                                    );
1043         }
1044 }
1045
1046 /// \brief \copydoc matrix4_get_rotation_euler_xyz(const Matrix4&)
1047 inline Vector3 matrix4_get_rotation_euler_xyz_degrees( const Matrix4& self ){
1048         return euler_radians_to_degrees( matrix4_get_rotation_euler_xyz( self ) );
1049 }
1050
1051 /// \brief Calculates and returns a set of euler angles that produce the rotation component of \p self when applied in the order (y, x, z).
1052 /// \p self must be affine and orthonormal (unscaled) to produce a meaningful result.
1053 inline Vector3 matrix4_get_rotation_euler_yxz( const Matrix4& self ){
1054         double a = asin( self[6] );
1055         double ca = cos( a );
1056
1057         if ( fabs( ca ) > 0.005 ) { // Gimbal lock?
1058                 return Vector3(
1059                                    static_cast<float>( a ),
1060                                    static_cast<float>( atan2( -self[2] / ca, self[10] / ca ) ),
1061                                    static_cast<float>( atan2( -self[4] / ca, self[5] / ca ) )
1062                                    );
1063         }
1064         else // Gimbal lock has occurred
1065         {
1066                 return Vector3(
1067                                    static_cast<float>( a ),
1068                                    static_cast<float>( atan2( self[8], self[0] ) ),
1069                                    0
1070                                    );
1071         }
1072 }
1073
1074 /// \brief \copydoc matrix4_get_rotation_euler_yxz(const Matrix4&)
1075 inline Vector3 matrix4_get_rotation_euler_yxz_degrees( const Matrix4& self ){
1076         return euler_radians_to_degrees( matrix4_get_rotation_euler_yxz( self ) );
1077 }
1078
1079 /// \brief Calculates and returns a set of euler angles that produce the rotation component of \p self when applied in the order (z, x, y).
1080 /// \p self must be affine and orthonormal (unscaled) to produce a meaningful result.
1081 inline Vector3 matrix4_get_rotation_euler_zxy( const Matrix4& self ){
1082         double a = asin( -self[9] );
1083         double ca = cos( a );
1084
1085         if ( fabs( ca ) > 0.005 ) { // Gimbal lock?
1086                 return Vector3(
1087                                    static_cast<float>( a ),
1088                                    static_cast<float>( atan2( self[8] / ca, self[10] / ca ) ),
1089                                    static_cast<float>( atan2( self[1] / ca, self[5] / ca ) )
1090                                    );
1091         }
1092         else // Gimbal lock has occurred
1093         {
1094                 return Vector3(
1095                                    static_cast<float>( a ),
1096                                    0,
1097                                    static_cast<float>( atan2( -self[4], self[0] ) )
1098                                    );
1099         }
1100 }
1101
1102 /// \brief \copydoc matrix4_get_rotation_euler_zxy(const Matrix4&)
1103 inline Vector3 matrix4_get_rotation_euler_zxy_degrees( const Matrix4& self ){
1104         return euler_radians_to_degrees( matrix4_get_rotation_euler_zxy( self ) );
1105 }
1106
1107 /// \brief Calculates and returns a set of euler angles that produce the rotation component of \p self when applied in the order (z, y, x).
1108 /// \p self must be affine and orthonormal (unscaled) to produce a meaningful result.
1109 inline Vector3 matrix4_get_rotation_euler_zyx( const Matrix4& self ){
1110         double a = asin( self[8] );
1111         double ca = cos( a );
1112
1113         if ( fabs( ca ) > 0.005 ) { // Gimbal lock?
1114                 return Vector3(
1115                                    static_cast<float>( atan2( -self[9] / ca, self[10] / ca ) ),
1116                                    static_cast<float>( a ),
1117                                    static_cast<float>( atan2( -self[4] / ca, self[0] / ca ) )
1118                                    );
1119         }
1120         else // Gimbal lock has occurred
1121         {
1122                 return Vector3(
1123                                    0,
1124                                    static_cast<float>( a ),
1125                                    static_cast<float>( atan2( self[1], self[5] ) )
1126                                    );
1127         }
1128 }
1129
1130 /// \brief \copydoc matrix4_get_rotation_euler_zyx(const Matrix4&)
1131 inline Vector3 matrix4_get_rotation_euler_zyx_degrees( const Matrix4& self ){
1132         return euler_radians_to_degrees( matrix4_get_rotation_euler_zyx( self ) );
1133 }
1134
1135
1136 /// \brief Rotate \p self by \p euler angles (degrees) applied in the order (x, y, z), using \p pivotpoint.
1137 inline void matrix4_pivoted_rotate_by_euler_xyz_degrees( Matrix4& self, const Vector3& euler, const Vector3& pivotpoint ){
1138         matrix4_translate_by_vec3( self, pivotpoint );
1139         matrix4_rotate_by_euler_xyz_degrees( self, euler );
1140         matrix4_translate_by_vec3( self, vector3_negated( pivotpoint ) );
1141 }
1142
1143
1144 /// \brief Constructs a pure-scale matrix from \p scale.
1145 inline Matrix4 matrix4_scale_for_vec3( const Vector3& scale ){
1146         return Matrix4(
1147                            scale[0], 0, 0, 0,
1148                            0, scale[1], 0, 0,
1149                            0, 0, scale[2], 0,
1150                            0, 0, 0,        1
1151                            );
1152 }
1153
1154 /// \brief Calculates and returns the (x, y, z) scale values that produce the scale component of \p self.
1155 /// \p self must be affine and orthogonal to produce a meaningful result.
1156 inline Vector3 matrix4_get_scale_vec3( const Matrix4& self ){
1157         return Vector3(
1158                            static_cast<float>( vector3_length( vector4_to_vector3( self.x() ) ) ),
1159                            static_cast<float>( vector3_length( vector4_to_vector3( self.y() ) ) ),
1160                            static_cast<float>( vector3_length( vector4_to_vector3( self.z() ) ) )
1161                            );
1162 }
1163
1164 /// \brief Scales \p self by \p scale.
1165 inline void matrix4_scale_by_vec3( Matrix4& self, const Vector3& scale ){
1166         matrix4_multiply_by_matrix4( self, matrix4_scale_for_vec3( scale ) );
1167 }
1168
1169 /// \brief Scales \p self by \p scale, using \p pivotpoint.
1170 inline void matrix4_pivoted_scale_by_vec3( Matrix4& self, const Vector3& scale, const Vector3& pivotpoint ){
1171         matrix4_translate_by_vec3( self, pivotpoint );
1172         matrix4_scale_by_vec3( self, scale );
1173         matrix4_translate_by_vec3( self, vector3_negated( pivotpoint ) );
1174 }
1175
1176
1177 /// \brief Transforms \p self by \p translation, \p euler and \p scale.
1178 /// The transforms are combined in the order: scale, rotate-z, rotate-y, rotate-x, translate.
1179 inline void matrix4_transform_by_euler_xyz_degrees( Matrix4& self, const Vector3& translation, const Vector3& euler, const Vector3& scale ){
1180         matrix4_translate_by_vec3( self, translation );
1181         matrix4_rotate_by_euler_xyz_degrees( self, euler );
1182         matrix4_scale_by_vec3( self, scale );
1183 }
1184
1185 /// \brief Transforms \p self by \p translation, \p euler and \p scale, using \p pivotpoint.
1186 inline void matrix4_pivoted_transform_by_euler_xyz_degrees( Matrix4& self, const Vector3& translation, const Vector3& euler, const Vector3& scale, const Vector3& pivotpoint ){
1187         matrix4_translate_by_vec3( self, pivotpoint + translation );
1188         matrix4_rotate_by_euler_xyz_degrees( self, euler );
1189         matrix4_scale_by_vec3( self, scale );
1190         matrix4_translate_by_vec3( self, vector3_negated( pivotpoint ) );
1191 }
1192
1193
1194 #endif