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