include/matrix4.h

   1 // Copyright (C) 2002-2012 Nikolaus Gebhardt\r
   2 // This file is part of the "Irrlicht Engine".\r
   3 // For conditions of distribution and use, see copyright notice in irrlicht.h\r
   4 \r
   5 #ifndef __IRR_MATRIX_H_INCLUDED__\r
   6 #define __IRR_MATRIX_H_INCLUDED__\r
   7 \r
   8 #include "irrMath.h"\r
   9 #include "vector3d.h"\r
  10 #include "vector2d.h"\r
  11 #include "plane3d.h"\r
  12 #include "aabbox3d.h"\r
  13 #include "rect.h"\r
  14 #include "irrString.h"\r
  15 \r
  16 // enable this to keep track of changes to the matrix\r
  17 // and make simpler identity check for seldom changing matrices\r
  18 // otherwise identity check will always compare the elements\r
  19 //#define USE_MATRIX_TEST\r
  20 \r
  21 // this is only for debugging purposes\r
  22 //#define USE_MATRIX_TEST_DEBUG\r
  23 \r
  24 #if defined( USE_MATRIX_TEST_DEBUG )\r
  25 \r
  26 struct MatrixTest\r
  27 {\r
  28         MatrixTest () : ID(0), Calls(0) {}\r
  29         char buf[256];\r
  30         int Calls;\r
  31         int ID;\r
  32 };\r
  33 static MatrixTest MTest;\r
  34 \r
  35 #endif\r
  36 \r
  37 namespace irr\r
  38 {\r
  39 namespace core\r
  40 {\r
  41 \r
  42         //! 4x4 matrix. Mostly used as transformation matrix for 3d calculations.\r
  43         /** The matrix is a D3D style matrix, row major with translations in the 4th row. */\r
  44         template <class T>\r
  45         class CMatrix4\r
  46         {\r
  47                 public:\r
  48 \r
  49                         //! Constructor Flags\r
  50                         enum eConstructor\r
  51                         {\r
  52                                 EM4CONST_NOTHING = 0,\r
  53                                 EM4CONST_COPY,\r
  54                                 EM4CONST_IDENTITY,\r
  55                                 EM4CONST_TRANSPOSED,\r
  56                                 EM4CONST_INVERSE,\r
  57                                 EM4CONST_INVERSE_TRANSPOSED\r
  58                         };\r
  59 \r
  60                         //! Default constructor\r
  61                         /** \param constructor Choose the initialization style */\r
  62                         CMatrix4( eConstructor constructor = EM4CONST_IDENTITY );\r
  63 \r
  64                         //! Constructor with value initialization\r
  65                         CMatrix4(const T& r0c0, const T& r0c1, const T& r0c2, const T& r0c3,\r
  66                                  const T& r1c0, const T& r1c1, const T& r1c2, const T& r1c3,\r
  67                                  const T& r2c0, const T& r2c1, const T& r2c2, const T& r2c3,\r
  68                                  const T& r3c0, const T& r3c1, const T& r3c2, const T& r3c3)\r
  69                         {\r
  70                                 M[0]  = r0c0; M[1]  = r0c1; M[2]  = r0c2; M[3]  = r0c3;\r
  71                                 M[4]  = r1c0; M[5]  = r1c1; M[6]  = r1c2; M[7]  = r1c3;\r
  72                                 M[8]  = r2c0; M[9]  = r2c1; M[10] = r2c2; M[11] = r2c3;\r
  73                                 M[12] = r3c0; M[13] = r3c1; M[14] = r3c2; M[15] = r3c3;\r
  74                         }\r
  75 \r
  76                         //! Copy constructor\r
  77                         /** \param other Other matrix to copy from\r
  78                         \param constructor Choose the initialization style */\r
  79                         CMatrix4(const CMatrix4<T>& other, eConstructor constructor = EM4CONST_COPY);\r
  80 \r
  81                         //! Simple operator for directly accessing every element of the matrix.\r
  82                         T& operator()(const s32 row, const s32 col)\r
  83                         {\r
  84 #if defined ( USE_MATRIX_TEST )\r
  85                                 definitelyIdentityMatrix=false;\r
  86 #endif\r
  87                                 return M[ row * 4 + col ];\r
  88                         }\r
  89 \r
  90                         //! Simple operator for directly accessing every element of the matrix.\r
  91                         const T& operator()(const s32 row, const s32 col) const { return M[row * 4 + col]; }\r
  92 \r
  93                         //! Simple operator for linearly accessing every element of the matrix.\r
  94                         T& operator[](u32 index)\r
  95                         {\r
  96 #if defined ( USE_MATRIX_TEST )\r
  97                                 definitelyIdentityMatrix=false;\r
  98 #endif\r
  99                                 return M[index];\r
 100                         }\r
 101 \r
 102                         //! Simple operator for linearly accessing every element of the matrix.\r
 103                         const T& operator[](u32 index) const { return M[index]; }\r
 104 \r
 105                         //! Sets this matrix equal to the other matrix.\r
 106                         CMatrix4<T>& operator=(const CMatrix4<T> &other) = default;\r
 107 \r
 108                         //! Sets all elements of this matrix to the value.\r
 109                         inline CMatrix4<T>& operator=(const T& scalar);\r
 110 \r
 111                         //! Returns pointer to internal array\r
 112                         const T* pointer() const { return M; }\r
 113                         T* pointer()\r
 114                         {\r
 115 #if defined ( USE_MATRIX_TEST )\r
 116                                 definitelyIdentityMatrix=false;\r
 117 #endif\r
 118                                 return M;\r
 119                         }\r
 120 \r
 121                         //! Returns true if other matrix is equal to this matrix.\r
 122                         bool operator==(const CMatrix4<T> &other) const;\r
 123 \r
 124                         //! Returns true if other matrix is not equal to this matrix.\r
 125                         bool operator!=(const CMatrix4<T> &other) const;\r
 126 \r
 127                         //! Add another matrix.\r
 128                         CMatrix4<T> operator+(const CMatrix4<T>& other) const;\r
 129 \r
 130                         //! Add another matrix.\r
 131                         CMatrix4<T>& operator+=(const CMatrix4<T>& other);\r
 132 \r
 133                         //! Subtract another matrix.\r
 134                         CMatrix4<T> operator-(const CMatrix4<T>& other) const;\r
 135 \r
 136                         //! Subtract another matrix.\r
 137                         CMatrix4<T>& operator-=(const CMatrix4<T>& other);\r
 138 \r
 139                         //! set this matrix to the product of two matrices\r
 140                         /** Calculate b*a */\r
 141                         inline CMatrix4<T>& setbyproduct(const CMatrix4<T>& other_a,const CMatrix4<T>& other_b );\r
 142 \r
 143                         //! Set this matrix to the product of two matrices\r
 144                         /** Calculate b*a, no optimization used,\r
 145                         use it if you know you never have a identity matrix */\r
 146                         CMatrix4<T>& setbyproduct_nocheck(const CMatrix4<T>& other_a,const CMatrix4<T>& other_b );\r
 147 \r
 148                         //! Multiply by another matrix.\r
 149                         /** Calculate other*this */\r
 150                         CMatrix4<T> operator*(const CMatrix4<T>& other) const;\r
 151 \r
 152                         //! Multiply by another matrix.\r
 153                         /** Calculate and return other*this */\r
 154                         CMatrix4<T>& operator*=(const CMatrix4<T>& other);\r
 155 \r
 156                         //! Multiply by scalar.\r
 157                         CMatrix4<T> operator*(const T& scalar) const;\r
 158 \r
 159                         //! Multiply by scalar.\r
 160                         CMatrix4<T>& operator*=(const T& scalar);\r
 161 \r
 162                         //! Set matrix to identity.\r
 163                         inline CMatrix4<T>& makeIdentity();\r
 164 \r
 165                         //! Returns true if the matrix is the identity matrix\r
 166                         inline bool isIdentity() const;\r
 167 \r
 168                         //! Returns true if the matrix is orthogonal\r
 169                         inline bool isOrthogonal() const;\r
 170 \r
 171                         //! Returns true if the matrix is the identity matrix\r
 172                         bool isIdentity_integer_base () const;\r
 173 \r
 174                         //! Set the translation of the current matrix. Will erase any previous values.\r
 175                         CMatrix4<T>& setTranslation( const vector3d<T>& translation );\r
 176 \r
 177                         //! Gets the current translation\r
 178                         vector3d<T> getTranslation() const;\r
 179 \r
 180                         //! Set the inverse translation of the current matrix. Will erase any previous values.\r
 181                         CMatrix4<T>& setInverseTranslation( const vector3d<T>& translation );\r
 182 \r
 183                         //! Make a rotation matrix from Euler angles. The 4th row and column are unmodified.\r
 184                         inline CMatrix4<T>& setRotationRadians( const vector3d<T>& rotation );\r
 185 \r
 186                         //! Make a rotation matrix from Euler angles. The 4th row and column are unmodified.\r
 187                         CMatrix4<T>& setRotationDegrees( const vector3d<T>& rotation );\r
 188 \r
 189                         //! Get the rotation, as set by setRotation() when you already know the scale.\r
 190                         /** If you already know the scale then this function is faster than the other getRotationDegrees overload.\r
 191                         NOTE: You will have the same end-rotation as used in setRotation, but it might not use the same axis values.\r
 192                         */\r
 193                         core::vector3d<T> getRotationDegrees(const vector3d<T>& scale) const;\r
 194 \r
 195                         //! Returns the rotation, as set by setRotation().\r
 196                         /** NOTE: You will have the same end-rotation as used in setRotation, but it might not use the same axis values.\r
 197                         */\r
 198                         core::vector3d<T> getRotationDegrees() const;\r
 199 \r
 200                         //! Make an inverted rotation matrix from Euler angles.\r
 201                         /** The 4th row and column are unmodified. */\r
 202                         inline CMatrix4<T>& setInverseRotationRadians( const vector3d<T>& rotation );\r
 203 \r
 204                         //! Make an inverted rotation matrix from Euler angles.\r
 205                         /** The 4th row and column are unmodified. */\r
 206                         inline CMatrix4<T>& setInverseRotationDegrees( const vector3d<T>& rotation );\r
 207 \r
 208                         //! Make a rotation matrix from angle and axis, assuming left handed rotation.\r
 209                         /** The 4th row and column are unmodified. */\r
 210                         inline CMatrix4<T>& setRotationAxisRadians(const T& angle, const vector3d<T>& axis);\r
 211 \r
 212                         //! Set Scale\r
 213                         CMatrix4<T>& setScale( const vector3d<T>& scale );\r
 214 \r
 215                         //! Set Scale\r
 216                         CMatrix4<T>& setScale( const T scale ) { return setScale(core::vector3d<T>(scale,scale,scale)); }\r
 217 \r
 218                         //! Get Scale\r
 219                         core::vector3d<T> getScale() const;\r
 220 \r
 221                         //! Translate a vector by the inverse of the translation part of this matrix.\r
 222                         void inverseTranslateVect( vector3df& vect ) const;\r
 223 \r
 224                         //! Rotate a vector by the inverse of the rotation part of this matrix.\r
 225                         void inverseRotateVect( vector3df& vect ) const;\r
 226 \r
 227                         //! Rotate a vector by the rotation part of this matrix.\r
 228                         void rotateVect( vector3df& vect ) const;\r
 229 \r
 230                         //! An alternate transform vector method, writing into a second vector\r
 231                         void rotateVect(core::vector3df& out, const core::vector3df& in) const;\r
 232 \r
 233                         //! An alternate transform vector method, writing into an array of 3 floats\r
 234                         void rotateVect(T *out,const core::vector3df &in) const;\r
 235 \r
 236                         //! Transforms the vector by this matrix\r
 237                         /** This operation is performed as if the vector was 4d with the 4th component =1 */\r
 238                         void transformVect( vector3df& vect) const;\r
 239 \r
 240                         //! Transforms input vector by this matrix and stores result in output vector\r
 241                         /** This operation is performed as if the vector was 4d with the 4th component =1 */\r
 242                         void transformVect( vector3df& out, const vector3df& in ) const;\r
 243 \r
 244                         //! An alternate transform vector method, writing into an array of 4 floats\r
 245                         /** This operation is performed as if the vector was 4d with the 4th component =1.\r
 246                                 NOTE: out[3] will be written to (4th vector component)*/\r
 247                         void transformVect(T *out,const core::vector3df &in) const;\r
 248 \r
 249                         //! An alternate transform vector method, reading from and writing to an array of 3 floats\r
 250                         /** This operation is performed as if the vector was 4d with the 4th component =1\r
 251                                 NOTE: out[3] will be written to (4th vector component)*/\r
 252                         void transformVec3(T *out, const T * in) const;\r
 253 \r
 254                         //! An alternate transform vector method, reading from and writing to an array of 4 floats\r
 255                         void transformVec4(T *out, const T * in) const;\r
 256 \r
 257                         //! Translate a vector by the translation part of this matrix.\r
 258                         /** This operation is performed as if the vector was 4d with the 4th component =1 */\r
 259                         void translateVect( vector3df& vect ) const;\r
 260 \r
 261                         //! Transforms a plane by this matrix\r
 262                         void transformPlane( core::plane3d<f32> &plane) const;\r
 263 \r
 264                         //! Transforms a plane by this matrix\r
 265                         void transformPlane( const core::plane3d<f32> &in, core::plane3d<f32> &out) const;\r
 266 \r
 267                         //! Transforms a axis aligned bounding box\r
 268                         /** The result box of this operation may not be accurate at all. For\r
 269                         correct results, use transformBoxEx() */\r
 270                         void transformBox(core::aabbox3d<f32>& box) const;\r
 271 \r
 272                         //! Transforms a axis aligned bounding box\r
 273                         /** The result box of this operation should be accurate, but this operation\r
 274                         is slower than transformBox(). */\r
 275                         void transformBoxEx(core::aabbox3d<f32>& box) const;\r
 276 \r
 277                         //! Multiplies this matrix by a 1x4 matrix\r
 278                         void multiplyWith1x4Matrix(T* matrix) const;\r
 279 \r
 280                         //! Calculates inverse of matrix. Slow.\r
 281                         /** \return Returns false if there is no inverse matrix.*/\r
 282                         bool makeInverse();\r
 283 \r
 284 \r
 285                         //! Inverts a primitive matrix which only contains a translation and a rotation\r
 286                         /** \param out: where result matrix is written to. */\r
 287                         bool getInversePrimitive ( CMatrix4<T>& out ) const;\r
 288 \r
 289                         //! Gets the inverse matrix of this one\r
 290                         /** \param out: where result matrix is written to.\r
 291                         \return Returns false if there is no inverse matrix. */\r
 292                         bool getInverse(CMatrix4<T>& out) const;\r
 293 \r
 294                         //! Builds a right-handed perspective projection matrix based on a field of view\r
 295                         //\param zClipFromZero: Clipping of z can be projected from 0 to w when true (D3D style) and from -w to w when false (OGL style).\r
 296                         CMatrix4<T>& buildProjectionMatrixPerspectiveFovRH(f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 zFar, bool zClipFromZero=true);\r
 297 \r
 298                         //! Builds a left-handed perspective projection matrix based on a field of view\r
 299                         CMatrix4<T>& buildProjectionMatrixPerspectiveFovLH(f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 zFar, bool zClipFromZero=true);\r
 300 \r
 301                         //! Builds a left-handed perspective projection matrix based on a field of view, with far plane at infinity\r
 302                         CMatrix4<T>& buildProjectionMatrixPerspectiveFovInfinityLH(f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 epsilon=0);\r
 303 \r
 304                         //! Builds a right-handed perspective projection matrix.\r
 305                         CMatrix4<T>& buildProjectionMatrixPerspectiveRH(f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero=true);\r
 306 \r
 307                         //! Builds a left-handed perspective projection matrix.\r
 308                         CMatrix4<T>& buildProjectionMatrixPerspectiveLH(f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero=true);\r
 309 \r
 310                         //! Builds a left-handed orthogonal projection matrix.\r
 311                         //\param zClipFromZero: Clipping of z can be projected from 0 to 1 when true (D3D style) and from -1 to 1 when false (OGL style).\r
 312                         CMatrix4<T>& buildProjectionMatrixOrthoLH(f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero=true);\r
 313 \r
 314                         //! Builds a right-handed orthogonal projection matrix.\r
 315                         CMatrix4<T>& buildProjectionMatrixOrthoRH(f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero=true);\r
 316 \r
 317                         //! Builds a left-handed look-at matrix.\r
 318                         CMatrix4<T>& buildCameraLookAtMatrixLH(\r
 319                                         const vector3df& position,\r
 320                                         const vector3df& target,\r
 321                                         const vector3df& upVector);\r
 322 \r
 323                         //! Builds a right-handed look-at matrix.\r
 324                         CMatrix4<T>& buildCameraLookAtMatrixRH(\r
 325                                         const vector3df& position,\r
 326                                         const vector3df& target,\r
 327                                         const vector3df& upVector);\r
 328 \r
 329                         //! Builds a matrix that flattens geometry into a plane.\r
 330                         /** \param light: light source\r
 331                         \param plane: plane into which the geometry if flattened into\r
 332                         \param point: value between 0 and 1, describing the light source.\r
 333                         If this is 1, it is a point light, if it is 0, it is a directional light. */\r
 334                         CMatrix4<T>& buildShadowMatrix(const core::vector3df& light, core::plane3df plane, f32 point=1.0f);\r
 335 \r
 336                         //! Builds a matrix which transforms a normalized Device Coordinate to Device Coordinates.\r
 337                         /** Used to scale <-1,-1><1,1> to viewport, for example from <-1,-1> <1,1> to the viewport <0,0><0,640> */\r
 338                         CMatrix4<T>& buildNDCToDCMatrix( const core::rect<s32>& area, f32 zScale);\r
 339 \r
 340                         //! Creates a new matrix as interpolated matrix from two other ones.\r
 341                         /** \param b: other matrix to interpolate with\r
 342                         \param time: Must be a value between 0 and 1. */\r
 343                         CMatrix4<T> interpolate(const core::CMatrix4<T>& b, f32 time) const;\r
 344 \r
 345                         //! Gets transposed matrix\r
 346                         CMatrix4<T> getTransposed() const;\r
 347 \r
 348                         //! Gets transposed matrix\r
 349                         inline void getTransposed( CMatrix4<T>& dest ) const;\r
 350 \r
 351                         //! Builds a matrix that rotates from one vector to another\r
 352                         /** \param from: vector to rotate from\r
 353                         \param to: vector to rotate to\r
 354                          */\r
 355                         CMatrix4<T>& buildRotateFromTo(const core::vector3df& from, const core::vector3df& to);\r
 356 \r
 357                         //! Builds a combined matrix which translates to a center before rotation and translates from origin afterwards\r
 358                         /** \param center Position to rotate around\r
 359                         \param translate Translation applied after the rotation\r
 360                          */\r
 361                         void setRotationCenter(const core::vector3df& center, const core::vector3df& translate);\r
 362 \r
 363                         //! Builds a matrix which rotates a source vector to a look vector over an arbitrary axis\r
 364                         /** \param camPos: viewer position in world coo\r
 365                         \param center: object position in world-coo and rotation pivot\r
 366                         \param translation: object final translation from center\r
 367                         \param axis: axis to rotate about\r
 368                         \param from: source vector to rotate from\r
 369                          */\r
 370                         void buildAxisAlignedBillboard(const core::vector3df& camPos,\r
 371                                                 const core::vector3df& center,\r
 372                                                 const core::vector3df& translation,\r
 373                                                 const core::vector3df& axis,\r
 374                                                 const core::vector3df& from);\r
 375 \r
 376                         /*\r
 377                                 construct 2D Texture transformations\r
 378                                 rotate about center, scale, and transform.\r
 379                         */\r
 380                         //! Set to a texture transformation matrix with the given parameters.\r
 381                         CMatrix4<T>& buildTextureTransform( f32 rotateRad,\r
 382                                         const core::vector2df &rotatecenter,\r
 383                                         const core::vector2df &translate,\r
 384                                         const core::vector2df &scale);\r
 385 \r
 386                         //! Set texture transformation rotation\r
 387                         /** Rotate about z axis, recenter at (0.5,0.5).\r
 388                         Doesn't clear other elements than those affected\r
 389                         \param radAngle Angle in radians\r
 390                         \return Altered matrix */\r
 391                         CMatrix4<T>& setTextureRotationCenter( f32 radAngle );\r
 392 \r
 393                         //! Set texture transformation translation\r
 394                         /** Doesn't clear other elements than those affected.\r
 395                         \param x Offset on x axis\r
 396                         \param y Offset on y axis\r
 397                         \return Altered matrix */\r
 398                         CMatrix4<T>& setTextureTranslate( f32 x, f32 y );\r
 399 \r
 400                         //! Get texture transformation translation\r
 401                         /** \param x returns offset on x axis\r
 402                         \param y returns offset on y axis */\r
 403                         void getTextureTranslate( f32& x, f32& y ) const;\r
 404 \r
 405                         //! Set texture transformation translation, using a transposed representation\r
 406                         /** Doesn't clear other elements than those affected.\r
 407                         \param x Offset on x axis\r
 408                         \param y Offset on y axis\r
 409                         \return Altered matrix */\r
 410                         CMatrix4<T>& setTextureTranslateTransposed( f32 x, f32 y );\r
 411 \r
 412                         //! Set texture transformation scale\r
 413                         /** Doesn't clear other elements than those affected.\r
 414                         \param sx Scale factor on x axis\r
 415                         \param sy Scale factor on y axis\r
 416                         \return Altered matrix. */\r
 417                         CMatrix4<T>& setTextureScale( f32 sx, f32 sy );\r
 418 \r
 419                         //! Get texture transformation scale\r
 420                         /** \param sx Returns x axis scale factor\r
 421                         \param sy Returns y axis scale factor */\r
 422                         void getTextureScale( f32& sx, f32& sy ) const;\r
 423 \r
 424                         //! Set texture transformation scale, and recenter at (0.5,0.5)\r
 425                         /** Doesn't clear other elements than those affected.\r
 426                         \param sx Scale factor on x axis\r
 427                         \param sy Scale factor on y axis\r
 428                         \return Altered matrix. */\r
 429                         CMatrix4<T>& setTextureScaleCenter( f32 sx, f32 sy );\r
 430 \r
 431                         //! Sets all matrix data members at once\r
 432                         CMatrix4<T>& setM(const T* data);\r
 433 \r
 434                         //! Sets if the matrix is definitely identity matrix\r
 435                         void setDefinitelyIdentityMatrix( bool isDefinitelyIdentityMatrix);\r
 436 \r
 437                         //! Gets if the matrix is definitely identity matrix\r
 438                         bool getDefinitelyIdentityMatrix() const;\r
 439 \r
 440                         //! Compare two matrices using the equal method\r
 441                         bool equals(const core::CMatrix4<T>& other, const T tolerance=(T)ROUNDING_ERROR_f64) const;\r
 442 \r
 443                 private:\r
 444                         //! Matrix data, stored in row-major order\r
 445                         T M[16];\r
 446 #if defined ( USE_MATRIX_TEST )\r
 447                         //! Flag is this matrix is identity matrix\r
 448                         mutable u32 definitelyIdentityMatrix;\r
 449 #endif\r
 450 #if defined ( USE_MATRIX_TEST_DEBUG )\r
 451                         u32 id;\r
 452                         mutable u32 calls;\r
 453 #endif\r
 454 \r
 455         };\r
 456 \r
 457         // Default constructor\r
 458         template <class T>\r
 459         inline CMatrix4<T>::CMatrix4( eConstructor constructor )\r
 460 #if defined ( USE_MATRIX_TEST )\r
 461                 : definitelyIdentityMatrix(BIT_UNTESTED)\r
 462 #endif\r
 463 #if defined ( USE_MATRIX_TEST_DEBUG )\r
 464                 ,id ( MTest.ID++), calls ( 0 )\r
 465 #endif\r
 466         {\r
 467                 switch ( constructor )\r
 468                 {\r
 469                         case EM4CONST_NOTHING:\r
 470                         case EM4CONST_COPY:\r
 471                                 break;\r
 472                         case EM4CONST_IDENTITY:\r
 473                         case EM4CONST_INVERSE:\r
 474                         default:\r
 475                                 makeIdentity();\r
 476                                 break;\r
 477                 }\r
 478         }\r
 479 \r
 480         // Copy constructor\r
 481         template <class T>\r
 482         inline CMatrix4<T>::CMatrix4( const CMatrix4<T>& other, eConstructor constructor)\r
 483 #if defined ( USE_MATRIX_TEST )\r
 484                 : definitelyIdentityMatrix(BIT_UNTESTED)\r
 485 #endif\r
 486 #if defined ( USE_MATRIX_TEST_DEBUG )\r
 487                 ,id ( MTest.ID++), calls ( 0 )\r
 488 #endif\r
 489         {\r
 490                 switch ( constructor )\r
 491                 {\r
 492                         case EM4CONST_IDENTITY:\r
 493                                 makeIdentity();\r
 494                                 break;\r
 495                         case EM4CONST_NOTHING:\r
 496                                 break;\r
 497                         case EM4CONST_COPY:\r
 498                                 *this = other;\r
 499                                 break;\r
 500                         case EM4CONST_TRANSPOSED:\r
 501                                 other.getTransposed(*this);\r
 502                                 break;\r
 503                         case EM4CONST_INVERSE:\r
 504                                 if (!other.getInverse(*this))\r
 505                                         memset(M, 0, 16*sizeof(T));\r
 506                                 break;\r
 507                         case EM4CONST_INVERSE_TRANSPOSED:\r
 508                                 if (!other.getInverse(*this))\r
 509                                         memset(M, 0, 16*sizeof(T));\r
 510                                 else\r
 511                                         *this=getTransposed();\r
 512                                 break;\r
 513                 }\r
 514         }\r
 515 \r
 516         //! Add another matrix.\r
 517         template <class T>\r
 518         inline CMatrix4<T> CMatrix4<T>::operator+(const CMatrix4<T>& other) const\r
 519         {\r
 520                 CMatrix4<T> temp ( EM4CONST_NOTHING );\r
 521 \r
 522                 temp[0] = M[0]+other[0];\r
 523                 temp[1] = M[1]+other[1];\r
 524                 temp[2] = M[2]+other[2];\r
 525                 temp[3] = M[3]+other[3];\r
 526                 temp[4] = M[4]+other[4];\r
 527                 temp[5] = M[5]+other[5];\r
 528                 temp[6] = M[6]+other[6];\r
 529                 temp[7] = M[7]+other[7];\r
 530                 temp[8] = M[8]+other[8];\r
 531                 temp[9] = M[9]+other[9];\r
 532                 temp[10] = M[10]+other[10];\r
 533                 temp[11] = M[11]+other[11];\r
 534                 temp[12] = M[12]+other[12];\r
 535                 temp[13] = M[13]+other[13];\r
 536                 temp[14] = M[14]+other[14];\r
 537                 temp[15] = M[15]+other[15];\r
 538 \r
 539                 return temp;\r
 540         }\r
 541 \r
 542         //! Add another matrix.\r
 543         template <class T>\r
 544         inline CMatrix4<T>& CMatrix4<T>::operator+=(const CMatrix4<T>& other)\r
 545         {\r
 546                 M[0]+=other[0];\r
 547                 M[1]+=other[1];\r
 548                 M[2]+=other[2];\r
 549                 M[3]+=other[3];\r
 550                 M[4]+=other[4];\r
 551                 M[5]+=other[5];\r
 552                 M[6]+=other[6];\r
 553                 M[7]+=other[7];\r
 554                 M[8]+=other[8];\r
 555                 M[9]+=other[9];\r
 556                 M[10]+=other[10];\r
 557                 M[11]+=other[11];\r
 558                 M[12]+=other[12];\r
 559                 M[13]+=other[13];\r
 560                 M[14]+=other[14];\r
 561                 M[15]+=other[15];\r
 562 \r
 563                 return *this;\r
 564         }\r
 565 \r
 566         //! Subtract another matrix.\r
 567         template <class T>\r
 568         inline CMatrix4<T> CMatrix4<T>::operator-(const CMatrix4<T>& other) const\r
 569         {\r
 570                 CMatrix4<T> temp ( EM4CONST_NOTHING );\r
 571 \r
 572                 temp[0] = M[0]-other[0];\r
 573                 temp[1] = M[1]-other[1];\r
 574                 temp[2] = M[2]-other[2];\r
 575                 temp[3] = M[3]-other[3];\r
 576                 temp[4] = M[4]-other[4];\r
 577                 temp[5] = M[5]-other[5];\r
 578                 temp[6] = M[6]-other[6];\r
 579                 temp[7] = M[7]-other[7];\r
 580                 temp[8] = M[8]-other[8];\r
 581                 temp[9] = M[9]-other[9];\r
 582                 temp[10] = M[10]-other[10];\r
 583                 temp[11] = M[11]-other[11];\r
 584                 temp[12] = M[12]-other[12];\r
 585                 temp[13] = M[13]-other[13];\r
 586                 temp[14] = M[14]-other[14];\r
 587                 temp[15] = M[15]-other[15];\r
 588 \r
 589                 return temp;\r
 590         }\r
 591 \r
 592         //! Subtract another matrix.\r
 593         template <class T>\r
 594         inline CMatrix4<T>& CMatrix4<T>::operator-=(const CMatrix4<T>& other)\r
 595         {\r
 596                 M[0]-=other[0];\r
 597                 M[1]-=other[1];\r
 598                 M[2]-=other[2];\r
 599                 M[3]-=other[3];\r
 600                 M[4]-=other[4];\r
 601                 M[5]-=other[5];\r
 602                 M[6]-=other[6];\r
 603                 M[7]-=other[7];\r
 604                 M[8]-=other[8];\r
 605                 M[9]-=other[9];\r
 606                 M[10]-=other[10];\r
 607                 M[11]-=other[11];\r
 608                 M[12]-=other[12];\r
 609                 M[13]-=other[13];\r
 610                 M[14]-=other[14];\r
 611                 M[15]-=other[15];\r
 612 \r
 613                 return *this;\r
 614         }\r
 615 \r
 616         //! Multiply by scalar.\r
 617         template <class T>\r
 618         inline CMatrix4<T> CMatrix4<T>::operator*(const T& scalar) const\r
 619         {\r
 620                 CMatrix4<T> temp ( EM4CONST_NOTHING );\r
 621 \r
 622                 temp[0] = M[0]*scalar;\r
 623                 temp[1] = M[1]*scalar;\r
 624                 temp[2] = M[2]*scalar;\r
 625                 temp[3] = M[3]*scalar;\r
 626                 temp[4] = M[4]*scalar;\r
 627                 temp[5] = M[5]*scalar;\r
 628                 temp[6] = M[6]*scalar;\r
 629                 temp[7] = M[7]*scalar;\r
 630                 temp[8] = M[8]*scalar;\r
 631                 temp[9] = M[9]*scalar;\r
 632                 temp[10] = M[10]*scalar;\r
 633                 temp[11] = M[11]*scalar;\r
 634                 temp[12] = M[12]*scalar;\r
 635                 temp[13] = M[13]*scalar;\r
 636                 temp[14] = M[14]*scalar;\r
 637                 temp[15] = M[15]*scalar;\r
 638 \r
 639                 return temp;\r
 640         }\r
 641 \r
 642         //! Multiply by scalar.\r
 643         template <class T>\r
 644         inline CMatrix4<T>& CMatrix4<T>::operator*=(const T& scalar)\r
 645         {\r
 646                 M[0]*=scalar;\r
 647                 M[1]*=scalar;\r
 648                 M[2]*=scalar;\r
 649                 M[3]*=scalar;\r
 650                 M[4]*=scalar;\r
 651                 M[5]*=scalar;\r
 652                 M[6]*=scalar;\r
 653                 M[7]*=scalar;\r
 654                 M[8]*=scalar;\r
 655                 M[9]*=scalar;\r
 656                 M[10]*=scalar;\r
 657                 M[11]*=scalar;\r
 658                 M[12]*=scalar;\r
 659                 M[13]*=scalar;\r
 660                 M[14]*=scalar;\r
 661                 M[15]*=scalar;\r
 662 \r
 663                 return *this;\r
 664         }\r
 665 \r
 666         //! Multiply by another matrix.\r
 667         template <class T>\r
 668         inline CMatrix4<T>& CMatrix4<T>::operator*=(const CMatrix4<T>& other)\r
 669         {\r
 670 #if defined ( USE_MATRIX_TEST )\r
 671                 // do checks on your own in order to avoid copy creation\r
 672                 if ( !other.isIdentity() )\r
 673                 {\r
 674                         if ( this->isIdentity() )\r
 675                         {\r
 676                                 return (*this = other);\r
 677                         }\r
 678                         else\r
 679                         {\r
 680                                 CMatrix4<T> temp ( *this );\r
 681                                 return setbyproduct_nocheck( temp, other );\r
 682                         }\r
 683                 }\r
 684                 return *this;\r
 685 #else\r
 686                 CMatrix4<T> temp ( *this );\r
 687                 return setbyproduct_nocheck( temp, other );\r
 688 #endif\r
 689         }\r
 690 \r
 691         //! multiply by another matrix\r
 692         // set this matrix to the product of two other matrices\r
 693         // goal is to reduce stack use and copy\r
 694         template <class T>\r
 695         inline CMatrix4<T>& CMatrix4<T>::setbyproduct_nocheck(const CMatrix4<T>& other_a,const CMatrix4<T>& other_b )\r
 696         {\r
 697                 const T *m1 = other_a.M;\r
 698                 const T *m2 = other_b.M;\r
 699 \r
 700                 M[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];\r
 701                 M[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];\r
 702                 M[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];\r
 703                 M[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];\r
 704 \r
 705                 M[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];\r
 706                 M[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];\r
 707                 M[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];\r
 708                 M[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];\r
 709 \r
 710                 M[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];\r
 711                 M[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];\r
 712                 M[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];\r
 713                 M[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];\r
 714 \r
 715                 M[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];\r
 716                 M[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];\r
 717                 M[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];\r
 718                 M[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];\r
 719 #if defined ( USE_MATRIX_TEST )\r
 720                 definitelyIdentityMatrix=false;\r
 721 #endif\r
 722                 return *this;\r
 723         }\r
 724 \r
 725 \r
 726         //! multiply by another matrix\r
 727         // set this matrix to the product of two other matrices\r
 728         // goal is to reduce stack use and copy\r
 729         template <class T>\r
 730         inline CMatrix4<T>& CMatrix4<T>::setbyproduct(const CMatrix4<T>& other_a, const CMatrix4<T>& other_b )\r
 731         {\r
 732 #if defined ( USE_MATRIX_TEST )\r
 733                 if ( other_a.isIdentity () )\r
 734                         return (*this = other_b);\r
 735                 else\r
 736                 if ( other_b.isIdentity () )\r
 737                         return (*this = other_a);\r
 738                 else\r
 739                         return setbyproduct_nocheck(other_a,other_b);\r
 740 #else\r
 741                 return setbyproduct_nocheck(other_a,other_b);\r
 742 #endif\r
 743         }\r
 744 \r
 745         //! multiply by another matrix\r
 746         template <class T>\r
 747         inline CMatrix4<T> CMatrix4<T>::operator*(const CMatrix4<T>& m2) const\r
 748         {\r
 749 #if defined ( USE_MATRIX_TEST )\r
 750                 // Testing purpose..\r
 751                 if ( this->isIdentity() )\r
 752                         return m2;\r
 753                 if ( m2.isIdentity() )\r
 754                         return *this;\r
 755 #endif\r
 756 \r
 757                 CMatrix4<T> m3 ( EM4CONST_NOTHING );\r
 758 \r
 759                 const T *m1 = M;\r
 760 \r
 761                 m3[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];\r
 762                 m3[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];\r
 763                 m3[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];\r
 764                 m3[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];\r
 765 \r
 766                 m3[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];\r
 767                 m3[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];\r
 768                 m3[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];\r
 769                 m3[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];\r
 770 \r
 771                 m3[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];\r
 772                 m3[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];\r
 773                 m3[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];\r
 774                 m3[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];\r
 775 \r
 776                 m3[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];\r
 777                 m3[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];\r
 778                 m3[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];\r
 779                 m3[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];\r
 780                 return m3;\r
 781         }\r
 782 \r
 783 \r
 784 \r
 785         template <class T>\r
 786         inline vector3d<T> CMatrix4<T>::getTranslation() const\r
 787         {\r
 788                 return vector3d<T>(M[12], M[13], M[14]);\r
 789         }\r
 790 \r
 791 \r
 792         template <class T>\r
 793         inline CMatrix4<T>& CMatrix4<T>::setTranslation( const vector3d<T>& translation )\r
 794         {\r
 795                 M[12] = translation.X;\r
 796                 M[13] = translation.Y;\r
 797                 M[14] = translation.Z;\r
 798 #if defined ( USE_MATRIX_TEST )\r
 799                 definitelyIdentityMatrix=false;\r
 800 #endif\r
 801                 return *this;\r
 802         }\r
 803 \r
 804         template <class T>\r
 805         inline CMatrix4<T>& CMatrix4<T>::setInverseTranslation( const vector3d<T>& translation )\r
 806         {\r
 807                 M[12] = -translation.X;\r
 808                 M[13] = -translation.Y;\r
 809                 M[14] = -translation.Z;\r
 810 #if defined ( USE_MATRIX_TEST )\r
 811                 definitelyIdentityMatrix=false;\r
 812 #endif\r
 813                 return *this;\r
 814         }\r
 815 \r
 816         template <class T>\r
 817         inline CMatrix4<T>& CMatrix4<T>::setScale( const vector3d<T>& scale )\r
 818         {\r
 819                 M[0] = scale.X;\r
 820                 M[5] = scale.Y;\r
 821                 M[10] = scale.Z;\r
 822 #if defined ( USE_MATRIX_TEST )\r
 823                 definitelyIdentityMatrix=false;\r
 824 #endif\r
 825                 return *this;\r
 826         }\r
 827 \r
 828         //! Returns the absolute values of the scales of the matrix.\r
 829         /**\r
 830         Note that this returns the absolute (positive) values unless only scale is set.\r
 831         Unfortunately it does not appear to be possible to extract any original negative\r
 832         values. The best that we could do would be to arbitrarily make one scale\r
 833         negative if one or three of them were negative.\r
 834         FIXME - return the original values.\r
 835         */\r
 836         template <class T>\r
 837         inline vector3d<T> CMatrix4<T>::getScale() const\r
 838         {\r
 839                 // See http://www.robertblum.com/articles/2005/02/14/decomposing-matrices\r
 840 \r
 841                 // Deal with the 0 rotation case first\r
 842                 // Prior to Irrlicht 1.6, we always returned this value.\r
 843                 if(core::iszero(M[1]) && core::iszero(M[2]) &&\r
 844                         core::iszero(M[4]) && core::iszero(M[6]) &&\r
 845                         core::iszero(M[8]) && core::iszero(M[9]))\r
 846                         return vector3d<T>(M[0], M[5], M[10]);\r
 847 \r
 848                 // We have to do the full calculation.\r
 849                 return vector3d<T>(sqrtf(M[0] * M[0] + M[1] * M[1] + M[2] * M[2]),\r
 850                                                         sqrtf(M[4] * M[4] + M[5] * M[5] + M[6] * M[6]),\r
 851                                                         sqrtf(M[8] * M[8] + M[9] * M[9] + M[10] * M[10]));\r
 852         }\r
 853 \r
 854         template <class T>\r
 855         inline CMatrix4<T>& CMatrix4<T>::setRotationDegrees( const vector3d<T>& rotation )\r
 856         {\r
 857                 return setRotationRadians( rotation * core::DEGTORAD );\r
 858         }\r
 859 \r
 860         template <class T>\r
 861         inline CMatrix4<T>& CMatrix4<T>::setInverseRotationDegrees( const vector3d<T>& rotation )\r
 862         {\r
 863                 return setInverseRotationRadians( rotation * core::DEGTORAD );\r
 864         }\r
 865 \r
 866         template <class T>\r
 867         inline CMatrix4<T>& CMatrix4<T>::setRotationRadians( const vector3d<T>& rotation )\r
 868         {\r
 869                 const f64 cr = cos( rotation.X );\r
 870                 const f64 sr = sin( rotation.X );\r
 871                 const f64 cp = cos( rotation.Y );\r
 872                 const f64 sp = sin( rotation.Y );\r
 873                 const f64 cy = cos( rotation.Z );\r
 874                 const f64 sy = sin( rotation.Z );\r
 875 \r
 876                 M[0] = (T)( cp*cy );\r
 877                 M[1] = (T)( cp*sy );\r
 878                 M[2] = (T)( -sp );\r
 879 \r
 880                 const f64 srsp = sr*sp;\r
 881                 const f64 crsp = cr*sp;\r
 882 \r
 883                 M[4] = (T)( srsp*cy-cr*sy );\r
 884                 M[5] = (T)( srsp*sy+cr*cy );\r
 885                 M[6] = (T)( sr*cp );\r
 886 \r
 887                 M[8] = (T)( crsp*cy+sr*sy );\r
 888                 M[9] = (T)( crsp*sy-sr*cy );\r
 889                 M[10] = (T)( cr*cp );\r
 890 #if defined ( USE_MATRIX_TEST )\r
 891                 definitelyIdentityMatrix=false;\r
 892 #endif\r
 893                 return *this;\r
 894         }\r
 895 \r
 896 \r
 897         //! Returns a rotation that is equivalent to that set by setRotationDegrees().\r
 898         /** This code was sent in by Chev.  Note that it does not necessarily return\r
 899         the *same* Euler angles as those set by setRotationDegrees(), but the rotation will\r
 900         be equivalent, i.e. will have the same result when used to rotate a vector or node.\r
 901         This code was originally written by by Chev.\r
 902         */\r
 903         template <class T>\r
 904         inline core::vector3d<T> CMatrix4<T>::getRotationDegrees(const vector3d<T>& scale_) const\r
 905         {\r
 906                 const CMatrix4<T> &mat = *this;\r
 907                 core::vector3d<T> scale(scale_);\r
 908                 // we need to check for negative scale on to axes, which would bring up wrong results\r
 909                 if (scale.Y<0 && scale.Z<0)\r
 910                 {\r
 911                         scale.Y =-scale.Y;\r
 912                         scale.Z =-scale.Z;\r
 913                 }\r
 914                 else if (scale.X<0 && scale.Z<0)\r
 915                 {\r
 916                         scale.X =-scale.X;\r
 917                         scale.Z =-scale.Z;\r
 918                 }\r
 919                 else if (scale.X<0 && scale.Y<0)\r
 920                 {\r
 921                         scale.X =-scale.X;\r
 922                         scale.Y =-scale.Y;\r
 923                 }\r
 924                 const core::vector3d<f64> invScale(core::reciprocal(scale.X),core::reciprocal(scale.Y),core::reciprocal(scale.Z));\r
 925 \r
 926                 f64 Y = -asin(core::clamp(mat[2]*invScale.X, -1.0, 1.0));\r
 927                 const f64 C = cos(Y);\r
 928                 Y *= RADTODEG64;\r
 929 \r
 930                 f64 rotx, roty, X, Z;\r
 931 \r
 932                 if (!core::iszero(C))\r
 933                 {\r
 934                         const f64 invC = core::reciprocal(C);\r
 935                         rotx = mat[10] * invC * invScale.Z;\r
 936                         roty = mat[6] * invC * invScale.Y;\r
 937                         X = atan2( roty, rotx ) * RADTODEG64;\r
 938                         rotx = mat[0] * invC * invScale.X;\r
 939                         roty = mat[1] * invC * invScale.X;\r
 940                         Z = atan2( roty, rotx ) * RADTODEG64;\r
 941                 }\r
 942                 else\r
 943                 {\r
 944                         X = 0.0;\r
 945                         rotx = mat[5] * invScale.Y;\r
 946                         roty = -mat[4] * invScale.Y;\r
 947                         Z = atan2( roty, rotx ) * RADTODEG64;\r
 948                 }\r
 949 \r
 950                 // fix values that get below zero\r
 951                 if (X < 0.0) X += 360.0;\r
 952                 if (Y < 0.0) Y += 360.0;\r
 953                 if (Z < 0.0) Z += 360.0;\r
 954 \r
 955                 return vector3d<T>((T)X,(T)Y,(T)Z);\r
 956         }\r
 957 \r
 958         //! Returns a rotation that is equivalent to that set by setRotationDegrees().\r
 959         /** This code was sent in by Chev.  Note that it does not necessarily return\r
 960         the *same* Euler angles as those set by setRotationDegrees(), but the rotation will\r
 961         be equivalent, i.e. will have the same result when used to rotate a vector or node.\r
 962         This code was originally written by by Chev. */\r
 963         template <class T>\r
 964         inline core::vector3d<T> CMatrix4<T>::getRotationDegrees() const\r
 965         {\r
 966                 return getRotationDegrees(getScale());\r
 967         }\r
 968 \r
 969 \r
 970         //! Sets matrix to rotation matrix of inverse angles given as parameters\r
 971         template <class T>\r
 972         inline CMatrix4<T>& CMatrix4<T>::setInverseRotationRadians( const vector3d<T>& rotation )\r
 973         {\r
 974                 f64 cr = cos( rotation.X );\r
 975                 f64 sr = sin( rotation.X );\r
 976                 f64 cp = cos( rotation.Y );\r
 977                 f64 sp = sin( rotation.Y );\r
 978                 f64 cy = cos( rotation.Z );\r
 979                 f64 sy = sin( rotation.Z );\r
 980 \r
 981                 M[0] = (T)( cp*cy );\r
 982                 M[4] = (T)( cp*sy );\r
 983                 M[8] = (T)( -sp );\r
 984 \r
 985                 f64 srsp = sr*sp;\r
 986                 f64 crsp = cr*sp;\r
 987 \r
 988                 M[1] = (T)( srsp*cy-cr*sy );\r
 989                 M[5] = (T)( srsp*sy+cr*cy );\r
 990                 M[9] = (T)( sr*cp );\r
 991 \r
 992                 M[2] = (T)( crsp*cy+sr*sy );\r
 993                 M[6] = (T)( crsp*sy-sr*cy );\r
 994                 M[10] = (T)( cr*cp );\r
 995 #if defined ( USE_MATRIX_TEST )\r
 996                 definitelyIdentityMatrix=false;\r
 997 #endif\r
 998                 return *this;\r
 999         }\r
1000 \r
1001         //! Sets matrix to rotation matrix defined by axis and angle, assuming LH rotation\r
1002         template <class T>\r
1003         inline CMatrix4<T>& CMatrix4<T>::setRotationAxisRadians( const T& angle, const vector3d<T>& axis )\r
1004         {\r
1005                 const f64 c = cos(angle);\r
1006                 const f64 s = sin(angle);\r
1007                 const f64 t = 1.0 - c;\r
1008 \r
1009                 const f64 tx  = t * axis.X;\r
1010                 const f64 ty  = t * axis.Y;\r
1011                 const f64 tz  = t * axis.Z;\r
1012 \r
1013                 const f64 sx  = s * axis.X;\r
1014                 const f64 sy  = s * axis.Y;\r
1015                 const f64 sz  = s * axis.Z;\r
1016 \r
1017                 M[0] = (T)(tx * axis.X + c);\r
1018                 M[1] = (T)(tx * axis.Y + sz);\r
1019                 M[2] = (T)(tx * axis.Z - sy);\r
1020 \r
1021                 M[4] = (T)(ty * axis.X - sz);\r
1022                 M[5] = (T)(ty * axis.Y + c);\r
1023                 M[6] = (T)(ty * axis.Z + sx);\r
1024 \r
1025                 M[8]  = (T)(tz * axis.X + sy);\r
1026                 M[9]  = (T)(tz * axis.Y - sx);\r
1027                 M[10] = (T)(tz * axis.Z + c);\r
1028 \r
1029 #if defined ( USE_MATRIX_TEST )\r
1030                 definitelyIdentityMatrix=false;\r
1031 #endif\r
1032                 return *this;\r
1033         }\r
1034 \r
1035 \r
1036         /*!\r
1037         */\r
1038         template <class T>\r
1039         inline CMatrix4<T>& CMatrix4<T>::makeIdentity()\r
1040         {\r
1041                 memset(M, 0, 16*sizeof(T));\r
1042                 M[0] = M[5] = M[10] = M[15] = (T)1;\r
1043 #if defined ( USE_MATRIX_TEST )\r
1044                 definitelyIdentityMatrix=true;\r
1045 #endif\r
1046                 return *this;\r
1047         }\r
1048 \r
1049 \r
1050         /*\r
1051                 check identity with epsilon\r
1052                 solve floating range problems..\r
1053         */\r
1054         template <class T>\r
1055         inline bool CMatrix4<T>::isIdentity() const\r
1056         {\r
1057 #if defined ( USE_MATRIX_TEST )\r
1058                 if (definitelyIdentityMatrix)\r
1059                         return true;\r
1060 #endif\r
1061                 if (!core::equals( M[12], (T)0 ) || !core::equals( M[13], (T)0 ) || !core::equals( M[14], (T)0 ) || !core::equals( M[15], (T)1 ))\r
1062                         return false;\r
1063 \r
1064                 if (!core::equals( M[ 0], (T)1 ) || !core::equals( M[ 1], (T)0 ) || !core::equals( M[ 2], (T)0 ) || !core::equals( M[ 3], (T)0 ))\r
1065                         return false;\r
1066 \r
1067                 if (!core::equals( M[ 4], (T)0 ) || !core::equals( M[ 5], (T)1 ) || !core::equals( M[ 6], (T)0 ) || !core::equals( M[ 7], (T)0 ))\r
1068                         return false;\r
1069 \r
1070                 if (!core::equals( M[ 8], (T)0 ) || !core::equals( M[ 9], (T)0 ) || !core::equals( M[10], (T)1 ) || !core::equals( M[11], (T)0 ))\r
1071                         return false;\r
1072 /*\r
1073                 if (!core::equals( M[ 0], (T)1 ) ||\r
1074                         !core::equals( M[ 5], (T)1 ) ||\r
1075                         !core::equals( M[10], (T)1 ) ||\r
1076                         !core::equals( M[15], (T)1 ))\r
1077                         return false;\r
1078 \r
1079                 for (s32 i=0; i<4; ++i)\r
1080                         for (s32 j=0; j<4; ++j)\r
1081                                 if ((j != i) && (!iszero((*this)(i,j))))\r
1082                                         return false;\r
1083 */\r
1084 #if defined ( USE_MATRIX_TEST )\r
1085                 definitelyIdentityMatrix=true;\r
1086 #endif\r
1087                 return true;\r
1088         }\r
1089 \r
1090 \r
1091         /* Check orthogonality of matrix. */\r
1092         template <class T>\r
1093         inline bool CMatrix4<T>::isOrthogonal() const\r
1094         {\r
1095                 T dp=M[0] * M[4 ] + M[1] * M[5 ] + M[2 ] * M[6 ] + M[3 ] * M[7 ];\r
1096                 if (!iszero(dp))\r
1097                         return false;\r
1098                 dp = M[0] * M[8 ] + M[1] * M[9 ] + M[2 ] * M[10] + M[3 ] * M[11];\r
1099                 if (!iszero(dp))\r
1100                         return false;\r
1101                 dp = M[0] * M[12] + M[1] * M[13] + M[2 ] * M[14] + M[3 ] * M[15];\r
1102                 if (!iszero(dp))\r
1103                         return false;\r
1104                 dp = M[4] * M[8 ] + M[5] * M[9 ] + M[6 ] * M[10] + M[7 ] * M[11];\r
1105                 if (!iszero(dp))\r
1106                         return false;\r
1107                 dp = M[4] * M[12] + M[5] * M[13] + M[6 ] * M[14] + M[7 ] * M[15];\r
1108                 if (!iszero(dp))\r
1109                         return false;\r
1110                 dp = M[8] * M[12] + M[9] * M[13] + M[10] * M[14] + M[11] * M[15];\r
1111                 return (iszero(dp));\r
1112         }\r
1113 \r
1114 \r
1115         /*\r
1116                 doesn't solve floating range problems..\r
1117                 but takes care on +/- 0 on translation because we are changing it..\r
1118                 reducing floating point branches\r
1119                 but it needs the floats in memory..\r
1120         */\r
1121         template <class T>\r
1122         inline bool CMatrix4<T>::isIdentity_integer_base() const\r
1123         {\r
1124 #if defined ( USE_MATRIX_TEST )\r
1125                 if (definitelyIdentityMatrix)\r
1126                         return true;\r
1127 #endif\r
1128                 if(IR(M[0])!=F32_VALUE_1)       return false;\r
1129                 if(IR(M[1])!=0)                 return false;\r
1130                 if(IR(M[2])!=0)                 return false;\r
1131                 if(IR(M[3])!=0)                 return false;\r
1132 \r
1133                 if(IR(M[4])!=0)                 return false;\r
1134                 if(IR(M[5])!=F32_VALUE_1)       return false;\r
1135                 if(IR(M[6])!=0)                 return false;\r
1136                 if(IR(M[7])!=0)                 return false;\r
1137 \r
1138                 if(IR(M[8])!=0)                 return false;\r
1139                 if(IR(M[9])!=0)                 return false;\r
1140                 if(IR(M[10])!=F32_VALUE_1)      return false;\r
1141                 if(IR(M[11])!=0)                return false;\r
1142 \r
1143                 if(IR(M[12])!=0)                return false;\r
1144                 if(IR(M[13])!=0)                return false;\r
1145                 if(IR(M[13])!=0)                return false;\r
1146                 if(IR(M[15])!=F32_VALUE_1)      return false;\r
1147 \r
1148 #if defined ( USE_MATRIX_TEST )\r
1149                 definitelyIdentityMatrix=true;\r
1150 #endif\r
1151                 return true;\r
1152         }\r
1153 \r
1154 \r
1155         template <class T>\r
1156         inline void CMatrix4<T>::rotateVect( vector3df& vect ) const\r
1157         {\r
1158                 vector3df tmp = vect;\r
1159                 vect.X = tmp.X*M[0] + tmp.Y*M[4] + tmp.Z*M[8];\r
1160                 vect.Y = tmp.X*M[1] + tmp.Y*M[5] + tmp.Z*M[9];\r
1161                 vect.Z = tmp.X*M[2] + tmp.Y*M[6] + tmp.Z*M[10];\r
1162         }\r
1163 \r
1164         //! An alternate transform vector method, writing into a second vector\r
1165         template <class T>\r
1166         inline void CMatrix4<T>::rotateVect(core::vector3df& out, const core::vector3df& in) const\r
1167         {\r
1168                 out.X = in.X*M[0] + in.Y*M[4] + in.Z*M[8];\r
1169                 out.Y = in.X*M[1] + in.Y*M[5] + in.Z*M[9];\r
1170                 out.Z = in.X*M[2] + in.Y*M[6] + in.Z*M[10];\r
1171         }\r
1172 \r
1173         //! An alternate transform vector method, writing into an array of 3 floats\r
1174         template <class T>\r
1175         inline void CMatrix4<T>::rotateVect(T *out, const core::vector3df& in) const\r
1176         {\r
1177                 out[0] = in.X*M[0] + in.Y*M[4] + in.Z*M[8];\r
1178                 out[1] = in.X*M[1] + in.Y*M[5] + in.Z*M[9];\r
1179                 out[2] = in.X*M[2] + in.Y*M[6] + in.Z*M[10];\r
1180         }\r
1181 \r
1182         template <class T>\r
1183         inline void CMatrix4<T>::inverseRotateVect( vector3df& vect ) const\r
1184         {\r
1185                 vector3df tmp = vect;\r
1186                 vect.X = tmp.X*M[0] + tmp.Y*M[1] + tmp.Z*M[2];\r
1187                 vect.Y = tmp.X*M[4] + tmp.Y*M[5] + tmp.Z*M[6];\r
1188                 vect.Z = tmp.X*M[8] + tmp.Y*M[9] + tmp.Z*M[10];\r
1189         }\r
1190 \r
1191         template <class T>\r
1192         inline void CMatrix4<T>::transformVect( vector3df& vect) const\r
1193         {\r
1194                 f32 vector[3];\r
1195 \r
1196                 vector[0] = vect.X*M[0] + vect.Y*M[4] + vect.Z*M[8] + M[12];\r
1197                 vector[1] = vect.X*M[1] + vect.Y*M[5] + vect.Z*M[9] + M[13];\r
1198                 vector[2] = vect.X*M[2] + vect.Y*M[6] + vect.Z*M[10] + M[14];\r
1199 \r
1200                 vect.X = vector[0];\r
1201                 vect.Y = vector[1];\r
1202                 vect.Z = vector[2];\r
1203         }\r
1204 \r
1205         template <class T>\r
1206         inline void CMatrix4<T>::transformVect( vector3df& out, const vector3df& in) const\r
1207         {\r
1208                 out.X = in.X*M[0] + in.Y*M[4] + in.Z*M[8] + M[12];\r
1209                 out.Y = in.X*M[1] + in.Y*M[5] + in.Z*M[9] + M[13];\r
1210                 out.Z = in.X*M[2] + in.Y*M[6] + in.Z*M[10] + M[14];\r
1211         }\r
1212 \r
1213 \r
1214         template <class T>\r
1215         inline void CMatrix4<T>::transformVect(T *out, const core::vector3df &in) const\r
1216         {\r
1217                 out[0] = in.X*M[0] + in.Y*M[4] + in.Z*M[8] + M[12];\r
1218                 out[1] = in.X*M[1] + in.Y*M[5] + in.Z*M[9] + M[13];\r
1219                 out[2] = in.X*M[2] + in.Y*M[6] + in.Z*M[10] + M[14];\r
1220                 out[3] = in.X*M[3] + in.Y*M[7] + in.Z*M[11] + M[15];\r
1221         }\r
1222 \r
1223         template <class T>\r
1224         inline void CMatrix4<T>::transformVec3(T *out, const T * in) const\r
1225         {\r
1226                 out[0] = in[0]*M[0] + in[1]*M[4] + in[2]*M[8] + M[12];\r
1227                 out[1] = in[0]*M[1] + in[1]*M[5] + in[2]*M[9] + M[13];\r
1228                 out[2] = in[0]*M[2] + in[1]*M[6] + in[2]*M[10] + M[14];\r
1229         }\r
1230 \r
1231         template <class T>\r
1232         inline void CMatrix4<T>::transformVec4(T *out, const T * in) const\r
1233         {\r
1234                 out[0] = in[0]*M[0] + in[1]*M[4] + in[2]*M[8] + in[3]*M[12];\r
1235                 out[1] = in[0]*M[1] + in[1]*M[5] + in[2]*M[9] + in[3]*M[13];\r
1236                 out[2] = in[0]*M[2] + in[1]*M[6] + in[2]*M[10] + in[3]*M[14];\r
1237                 out[3] = in[0]*M[3] + in[1]*M[7] + in[2]*M[11] + in[3]*M[15];\r
1238         }\r
1239 \r
1240 \r
1241         //! Transforms a plane by this matrix\r
1242         template <class T>\r
1243         inline void CMatrix4<T>::transformPlane( core::plane3d<f32> &plane) const\r
1244         {\r
1245                 vector3df member;\r
1246                 // Transform the plane member point, i.e. rotate, translate and scale it.\r
1247                 transformVect(member, plane.getMemberPoint());\r
1248 \r
1249                 // Transform the normal by the transposed inverse of the matrix\r
1250                 CMatrix4<T> transposedInverse(*this, EM4CONST_INVERSE_TRANSPOSED);\r
1251                 vector3df normal = plane.Normal;\r
1252                 transposedInverse.rotateVect(normal);\r
1253                 plane.setPlane(member, normal.normalize());\r
1254         }\r
1255 \r
1256         //! Transforms a plane by this matrix\r
1257         template <class T>\r
1258         inline void CMatrix4<T>::transformPlane( const core::plane3d<f32> &in, core::plane3d<f32> &out) const\r
1259         {\r
1260                 out = in;\r
1261                 transformPlane( out );\r
1262         }\r
1263 \r
1264         //! Transforms the edge-points of a bounding box\r
1265         //! Deprecated as it's usually not what people need (regards only 2 corners, but other corners might be outside the box after transformation)\r
1266         //! Use transformBoxEx instead.\r
1267         template <class T>\r
1268         _IRR_DEPRECATED_ inline void CMatrix4<T>::transformBox(core::aabbox3d<f32>& box) const\r
1269         {\r
1270 #if defined ( USE_MATRIX_TEST )\r
1271                 if (isIdentity())\r
1272                         return;\r
1273 #endif\r
1274 \r
1275                 transformVect(box.MinEdge);\r
1276                 transformVect(box.MaxEdge);\r
1277                 box.repair();\r
1278         }\r
1279 \r
1280         //! Transforms a axis aligned bounding box more accurately than transformBox()\r
1281         template <class T>\r
1282         inline void CMatrix4<T>::transformBoxEx(core::aabbox3d<f32>& box) const\r
1283         {\r
1284 #if defined ( USE_MATRIX_TEST )\r
1285                 if (isIdentity())\r
1286                         return;\r
1287 #endif\r
1288 \r
1289                 const f32 Amin[3] = {box.MinEdge.X, box.MinEdge.Y, box.MinEdge.Z};\r
1290                 const f32 Amax[3] = {box.MaxEdge.X, box.MaxEdge.Y, box.MaxEdge.Z};\r
1291 \r
1292                 f32 Bmin[3];\r
1293                 f32 Bmax[3];\r
1294 \r
1295                 Bmin[0] = Bmax[0] = M[12];\r
1296                 Bmin[1] = Bmax[1] = M[13];\r
1297                 Bmin[2] = Bmax[2] = M[14];\r
1298 \r
1299                 const CMatrix4<T> &m = *this;\r
1300 \r
1301                 for (u32 i = 0; i < 3; ++i)\r
1302                 {\r
1303                         for (u32 j = 0; j < 3; ++j)\r
1304                         {\r
1305                                 const f32 a = m(j,i) * Amin[j];\r
1306                                 const f32 b = m(j,i) * Amax[j];\r
1307 \r
1308                                 if (a < b)\r
1309                                 {\r
1310                                         Bmin[i] += a;\r
1311                                         Bmax[i] += b;\r
1312                                 }\r
1313                                 else\r
1314                                 {\r
1315                                         Bmin[i] += b;\r
1316                                         Bmax[i] += a;\r
1317                                 }\r
1318                         }\r
1319                 }\r
1320 \r
1321                 box.MinEdge.X = Bmin[0];\r
1322                 box.MinEdge.Y = Bmin[1];\r
1323                 box.MinEdge.Z = Bmin[2];\r
1324 \r
1325                 box.MaxEdge.X = Bmax[0];\r
1326                 box.MaxEdge.Y = Bmax[1];\r
1327                 box.MaxEdge.Z = Bmax[2];\r
1328         }\r
1329 \r
1330 \r
1331         //! Multiplies this matrix by a 1x4 matrix\r
1332         template <class T>\r
1333         inline void CMatrix4<T>::multiplyWith1x4Matrix(T* matrix) const\r
1334         {\r
1335                 /*\r
1336                 0  1  2  3\r
1337                 4  5  6  7\r
1338                 8  9  10 11\r
1339                 12 13 14 15\r
1340                 */\r
1341 \r
1342                 T mat[4];\r
1343                 mat[0] = matrix[0];\r
1344                 mat[1] = matrix[1];\r
1345                 mat[2] = matrix[2];\r
1346                 mat[3] = matrix[3];\r
1347 \r
1348                 matrix[0] = M[0]*mat[0] + M[4]*mat[1] + M[8]*mat[2] + M[12]*mat[3];\r
1349                 matrix[1] = M[1]*mat[0] + M[5]*mat[1] + M[9]*mat[2] + M[13]*mat[3];\r
1350                 matrix[2] = M[2]*mat[0] + M[6]*mat[1] + M[10]*mat[2] + M[14]*mat[3];\r
1351                 matrix[3] = M[3]*mat[0] + M[7]*mat[1] + M[11]*mat[2] + M[15]*mat[3];\r
1352         }\r
1353 \r
1354         template <class T>\r
1355         inline void CMatrix4<T>::inverseTranslateVect( vector3df& vect ) const\r
1356         {\r
1357                 vect.X = vect.X-M[12];\r
1358                 vect.Y = vect.Y-M[13];\r
1359                 vect.Z = vect.Z-M[14];\r
1360         }\r
1361 \r
1362         template <class T>\r
1363         inline void CMatrix4<T>::translateVect( vector3df& vect ) const\r
1364         {\r
1365                 vect.X = vect.X+M[12];\r
1366                 vect.Y = vect.Y+M[13];\r
1367                 vect.Z = vect.Z+M[14];\r
1368         }\r
1369 \r
1370 \r
1371         template <class T>\r
1372         inline bool CMatrix4<T>::getInverse(CMatrix4<T>& out) const\r
1373         {\r
1374                 /// Calculates the inverse of this Matrix\r
1375                 /// The inverse is calculated using Cramers rule.\r
1376                 /// If no inverse exists then 'false' is returned.\r
1377 \r
1378 #if defined ( USE_MATRIX_TEST )\r
1379                 if ( this->isIdentity() )\r
1380                 {\r
1381                         out=*this;\r
1382                         return true;\r
1383                 }\r
1384 #endif\r
1385                 const CMatrix4<T> &m = *this;\r
1386 \r
1387                 f32 d = (m[0] * m[5] - m[1] * m[4]) * (m[10] * m[15] - m[11] * m[14]) -\r
1388                         (m[0] * m[6] - m[2] * m[4]) * (m[9] * m[15] - m[11] * m[13]) +\r
1389                         (m[0] * m[7] - m[3] * m[4]) * (m[9] * m[14] - m[10] * m[13]) +\r
1390                         (m[1] * m[6] - m[2] * m[5]) * (m[8] * m[15] - m[11] * m[12]) -\r
1391                         (m[1] * m[7] - m[3] * m[5]) * (m[8] * m[14] - m[10] * m[12]) +\r
1392                         (m[2] * m[7] - m[3] * m[6]) * (m[8] * m[13] - m[9] * m[12]);\r
1393 \r
1394                 if( core::iszero ( d, FLT_MIN ) )\r
1395                         return false;\r
1396 \r
1397                 d = core::reciprocal ( d );\r
1398 \r
1399                 out[0] = d * (m[5] * (m[10] * m[15] - m[11] * m[14]) +\r
1400                                 m[6] * (m[11] * m[13] - m[9] * m[15]) +\r
1401                                 m[7] * (m[9] * m[14] - m[10] * m[13]));\r
1402                 out[1] = d * (m[9] * (m[2] * m[15] - m[3] * m[14]) +\r
1403                                 m[10] * (m[3] * m[13] - m[1] * m[15]) +\r
1404                                 m[11] * (m[1] * m[14] - m[2] * m[13]));\r
1405                 out[2] = d * (m[13] * (m[2] * m[7] - m[3] * m[6]) +\r
1406                                 m[14] * (m[3] * m[5] - m[1] * m[7]) +\r
1407                                 m[15] * (m[1] * m[6] - m[2] * m[5]));\r
1408                 out[3] = d * (m[1] * (m[7] * m[10] - m[6] * m[11]) +\r
1409                                 m[2] * (m[5] * m[11] - m[7] * m[9]) +\r
1410                                 m[3] * (m[6] * m[9] - m[5] * m[10]));\r
1411                 out[4] = d * (m[6] * (m[8] * m[15] - m[11] * m[12]) +\r
1412                                 m[7] * (m[10] * m[12] - m[8] * m[14]) +\r
1413                                 m[4] * (m[11] * m[14] - m[10] * m[15]));\r
1414                 out[5] = d * (m[10] * (m[0] * m[15] - m[3] * m[12]) +\r
1415                                 m[11] * (m[2] * m[12] - m[0] * m[14]) +\r
1416                                 m[8] * (m[3] * m[14] - m[2] * m[15]));\r
1417                 out[6] = d * (m[14] * (m[0] * m[7] - m[3] * m[4]) +\r
1418                                 m[15] * (m[2] * m[4] - m[0] * m[6]) +\r
1419                                 m[12] * (m[3] * m[6] - m[2] * m[7]));\r
1420                 out[7] = d * (m[2] * (m[7] * m[8] - m[4] * m[11]) +\r
1421                                 m[3] * (m[4] * m[10] - m[6] * m[8]) +\r
1422                                 m[0] * (m[6] * m[11] - m[7] * m[10]));\r
1423                 out[8] = d * (m[7] * (m[8] * m[13] - m[9] * m[12]) +\r
1424                                 m[4] * (m[9] * m[15] - m[11] * m[13]) +\r
1425                                 m[5] * (m[11] * m[12] - m[8] * m[15]));\r
1426                 out[9] = d * (m[11] * (m[0] * m[13] - m[1] * m[12]) +\r
1427                                 m[8] * (m[1] * m[15] - m[3] * m[13]) +\r
1428                                 m[9] * (m[3] * m[12] - m[0] * m[15]));\r
1429                 out[10] = d * (m[15] * (m[0] * m[5] - m[1] * m[4]) +\r
1430                                 m[12] * (m[1] * m[7] - m[3] * m[5]) +\r
1431                                 m[13] * (m[3] * m[4] - m[0] * m[7]));\r
1432                 out[11] = d * (m[3] * (m[5] * m[8] - m[4] * m[9]) +\r
1433                                 m[0] * (m[7] * m[9] - m[5] * m[11]) +\r
1434                                 m[1] * (m[4] * m[11] - m[7] * m[8]));\r
1435                 out[12] = d * (m[4] * (m[10] * m[13] - m[9] * m[14]) +\r
1436                                 m[5] * (m[8] * m[14] - m[10] * m[12]) +\r
1437                                 m[6] * (m[9] * m[12] - m[8] * m[13]));\r
1438                 out[13] = d * (m[8] * (m[2] * m[13] - m[1] * m[14]) +\r
1439                                 m[9] * (m[0] * m[14] - m[2] * m[12]) +\r
1440                                 m[10] * (m[1] * m[12] - m[0] * m[13]));\r
1441                 out[14] = d * (m[12] * (m[2] * m[5] - m[1] * m[6]) +\r
1442                                 m[13] * (m[0] * m[6] - m[2] * m[4]) +\r
1443                                 m[14] * (m[1] * m[4] - m[0] * m[5]));\r
1444                 out[15] = d * (m[0] * (m[5] * m[10] - m[6] * m[9]) +\r
1445                                 m[1] * (m[6] * m[8] - m[4] * m[10]) +\r
1446                                 m[2] * (m[4] * m[9] - m[5] * m[8]));\r
1447 \r
1448 #if defined ( USE_MATRIX_TEST )\r
1449                 out.definitelyIdentityMatrix = definitelyIdentityMatrix;\r
1450 #endif\r
1451                 return true;\r
1452         }\r
1453 \r
1454 \r
1455         //! Inverts a primitive matrix which only contains a translation and a rotation\r
1456         //! \param out: where result matrix is written to.\r
1457         template <class T>\r
1458         inline bool CMatrix4<T>::getInversePrimitive ( CMatrix4<T>& out ) const\r
1459         {\r
1460                 out.M[0 ] = M[0];\r
1461                 out.M[1 ] = M[4];\r
1462                 out.M[2 ] = M[8];\r
1463                 out.M[3 ] = 0;\r
1464 \r
1465                 out.M[4 ] = M[1];\r
1466                 out.M[5 ] = M[5];\r
1467                 out.M[6 ] = M[9];\r
1468                 out.M[7 ] = 0;\r
1469 \r
1470                 out.M[8 ] = M[2];\r
1471                 out.M[9 ] = M[6];\r
1472                 out.M[10] = M[10];\r
1473                 out.M[11] = 0;\r
1474 \r
1475                 out.M[12] = (T)-(M[12]*M[0] + M[13]*M[1] + M[14]*M[2]);\r
1476                 out.M[13] = (T)-(M[12]*M[4] + M[13]*M[5] + M[14]*M[6]);\r
1477                 out.M[14] = (T)-(M[12]*M[8] + M[13]*M[9] + M[14]*M[10]);\r
1478                 out.M[15] = 1;\r
1479 \r
1480 #if defined ( USE_MATRIX_TEST )\r
1481                 out.definitelyIdentityMatrix = definitelyIdentityMatrix;\r
1482 #endif\r
1483                 return true;\r
1484         }\r
1485 \r
1486         /*!\r
1487         */\r
1488         template <class T>\r
1489         inline bool CMatrix4<T>::makeInverse()\r
1490         {\r
1491 #if defined ( USE_MATRIX_TEST )\r
1492                 if (definitelyIdentityMatrix)\r
1493                         return true;\r
1494 #endif\r
1495                 CMatrix4<T> temp ( EM4CONST_NOTHING );\r
1496 \r
1497                 if (getInverse(temp))\r
1498                 {\r
1499                         *this = temp;\r
1500                         return true;\r
1501                 }\r
1502 \r
1503                 return false;\r
1504         }\r
1505 \r
1506 \r
1507         template <class T>\r
1508         inline CMatrix4<T>& CMatrix4<T>::operator=(const T& scalar)\r
1509         {\r
1510                 for (s32 i = 0; i < 16; ++i)\r
1511                         M[i]=scalar;\r
1512 \r
1513 #if defined ( USE_MATRIX_TEST )\r
1514                 definitelyIdentityMatrix=false;\r
1515 #endif\r
1516                 return *this;\r
1517         }\r
1518 \r
1519 \r
1520         template <class T>\r
1521         inline bool CMatrix4<T>::operator==(const CMatrix4<T> &other) const\r
1522         {\r
1523 #if defined ( USE_MATRIX_TEST )\r
1524                 if (definitelyIdentityMatrix && other.definitelyIdentityMatrix)\r
1525                         return true;\r
1526 #endif\r
1527                 for (s32 i = 0; i < 16; ++i)\r
1528                         if (M[i] != other.M[i])\r
1529                                 return false;\r
1530 \r
1531                 return true;\r
1532         }\r
1533 \r
1534 \r
1535         template <class T>\r
1536         inline bool CMatrix4<T>::operator!=(const CMatrix4<T> &other) const\r
1537         {\r
1538                 return !(*this == other);\r
1539         }\r
1540 \r
1541 \r
1542         // Builds a right-handed perspective projection matrix based on a field of view\r
1543         template <class T>\r
1544         inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveFovRH(\r
1545                         f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 zFar, bool zClipFromZero)\r
1546         {\r
1547                 const f64 h = reciprocal(tan(fieldOfViewRadians*0.5));\r
1548                 _IRR_DEBUG_BREAK_IF(aspectRatio==0.f); //divide by zero\r
1549                 const T w = static_cast<T>(h / aspectRatio);\r
1550 \r
1551                 _IRR_DEBUG_BREAK_IF(zNear==zFar); //divide by zero\r
1552                 M[0] = w;\r
1553                 M[1] = 0;\r
1554                 M[2] = 0;\r
1555                 M[3] = 0;\r
1556 \r
1557                 M[4] = 0;\r
1558                 M[5] = (T)h;\r
1559                 M[6] = 0;\r
1560                 M[7] = 0;\r
1561 \r
1562                 M[8] = 0;\r
1563                 M[9] = 0;\r
1564                 //M[10]\r
1565                 M[11] = -1;\r
1566 \r
1567                 M[12] = 0;\r
1568                 M[13] = 0;\r
1569                 //M[14]\r
1570                 M[15] = 0;\r
1571 \r
1572                 if ( zClipFromZero ) // DirectX version\r
1573                 {\r
1574                         M[10] = (T)(zFar/(zNear-zFar));\r
1575                         M[14] = (T)(zNear*zFar/(zNear-zFar));\r
1576                 }\r
1577                 else    // OpenGL version\r
1578                 {\r
1579                         M[10] = (T)((zFar+zNear)/(zNear-zFar));\r
1580                         M[14] = (T)(2.0f*zNear*zFar/(zNear-zFar));\r
1581                 }\r
1582 \r
1583 #if defined ( USE_MATRIX_TEST )\r
1584                 definitelyIdentityMatrix=false;\r
1585 #endif\r
1586                 return *this;\r
1587         }\r
1588 \r
1589 \r
1590         // Builds a left-handed perspective projection matrix based on a field of view\r
1591         template <class T>\r
1592         inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveFovLH(\r
1593                         f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 zFar, bool zClipFromZero)\r
1594         {\r
1595                 const f64 h = reciprocal(tan(fieldOfViewRadians*0.5));\r
1596                 _IRR_DEBUG_BREAK_IF(aspectRatio==0.f); //divide by zero\r
1597                 const T w = static_cast<T>(h / aspectRatio);\r
1598 \r
1599                 _IRR_DEBUG_BREAK_IF(zNear==zFar); //divide by zero\r
1600                 M[0] = w;\r
1601                 M[1] = 0;\r
1602                 M[2] = 0;\r
1603                 M[3] = 0;\r
1604 \r
1605                 M[4] = 0;\r
1606                 M[5] = (T)h;\r
1607                 M[6] = 0;\r
1608                 M[7] = 0;\r
1609 \r
1610                 M[8] = 0;\r
1611                 M[9] = 0;\r
1612                 //M[10]\r
1613                 M[11] = 1;\r
1614 \r
1615                 M[12] = 0;\r
1616                 M[13] = 0;\r
1617                 //M[14]\r
1618                 M[15] = 0;\r
1619 \r
1620                 if ( zClipFromZero ) // DirectX version\r
1621                 {\r
1622                         M[10] = (T)(zFar/(zFar-zNear));\r
1623                         M[14] = (T)(-zNear*zFar/(zFar-zNear));\r
1624                 }\r
1625                 else    // OpenGL version\r
1626                 {\r
1627                         M[10] = (T)((zFar+zNear)/(zFar-zNear));\r
1628                         M[14] = (T)(2.0f*zNear*zFar/(zNear-zFar));\r
1629                 }\r
1630 \r
1631 #if defined ( USE_MATRIX_TEST )\r
1632                 definitelyIdentityMatrix=false;\r
1633 #endif\r
1634                 return *this;\r
1635         }\r
1636 \r
1637 \r
1638         // Builds a left-handed perspective projection matrix based on a field of view, with far plane culling at infinity\r
1639         template <class T>\r
1640         inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveFovInfinityLH(\r
1641                         f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 epsilon)\r
1642         {\r
1643                 const f64 h = reciprocal(tan(fieldOfViewRadians*0.5));\r
1644                 _IRR_DEBUG_BREAK_IF(aspectRatio==0.f); //divide by zero\r
1645                 const T w = static_cast<T>(h / aspectRatio);\r
1646 \r
1647                 M[0] = w;\r
1648                 M[1] = 0;\r
1649                 M[2] = 0;\r
1650                 M[3] = 0;\r
1651 \r
1652                 M[4] = 0;\r
1653                 M[5] = (T)h;\r
1654                 M[6] = 0;\r
1655                 M[7] = 0;\r
1656 \r
1657                 M[8] = 0;\r
1658                 M[9] = 0;\r
1659                 M[10] = (T)(1.f-epsilon);\r
1660                 M[11] = 1;\r
1661 \r
1662                 M[12] = 0;\r
1663                 M[13] = 0;\r
1664                 M[14] = (T)(zNear*(epsilon-1.f));\r
1665                 M[15] = 0;\r
1666 \r
1667 #if defined ( USE_MATRIX_TEST )\r
1668                 definitelyIdentityMatrix=false;\r
1669 #endif\r
1670                 return *this;\r
1671         }\r
1672 \r
1673 \r
1674         // Builds a left-handed orthogonal projection matrix.\r
1675         template <class T>\r
1676         inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixOrthoLH(\r
1677                         f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero)\r
1678         {\r
1679                 _IRR_DEBUG_BREAK_IF(widthOfViewVolume==0.f); //divide by zero\r
1680                 _IRR_DEBUG_BREAK_IF(heightOfViewVolume==0.f); //divide by zero\r
1681                 _IRR_DEBUG_BREAK_IF(zNear==zFar); //divide by zero\r
1682                 M[0] = (T)(2/widthOfViewVolume);\r
1683                 M[1] = 0;\r
1684                 M[2] = 0;\r
1685                 M[3] = 0;\r
1686 \r
1687                 M[4] = 0;\r
1688                 M[5] = (T)(2/heightOfViewVolume);\r
1689                 M[6] = 0;\r
1690                 M[7] = 0;\r
1691 \r
1692                 M[8] = 0;\r
1693                 M[9] = 0;\r
1694                 // M[10]\r
1695                 M[11] = 0;\r
1696 \r
1697                 M[12] = 0;\r
1698                 M[13] = 0;\r
1699                 // M[14]\r
1700                 M[15] = 1;\r
1701 \r
1702                 if ( zClipFromZero )\r
1703                 {\r
1704                         M[10] = (T)(1/(zFar-zNear));\r
1705                         M[14] = (T)(zNear/(zNear-zFar));\r
1706                 }\r
1707                 else\r
1708                 {\r
1709                         M[10] = (T)(2/(zFar-zNear));\r
1710                         M[14] = (T)-(zFar+zNear)/(zFar-zNear);\r
1711                 }\r
1712 \r
1713 #if defined ( USE_MATRIX_TEST )\r
1714                 definitelyIdentityMatrix=false;\r
1715 #endif\r
1716                 return *this;\r
1717         }\r
1718 \r
1719 \r
1720         // Builds a right-handed orthogonal projection matrix.\r
1721         template <class T>\r
1722         inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixOrthoRH(\r
1723                         f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero)\r
1724         {\r
1725                 _IRR_DEBUG_BREAK_IF(widthOfViewVolume==0.f); //divide by zero\r
1726                 _IRR_DEBUG_BREAK_IF(heightOfViewVolume==0.f); //divide by zero\r
1727                 _IRR_DEBUG_BREAK_IF(zNear==zFar); //divide by zero\r
1728                 M[0] = (T)(2/widthOfViewVolume);\r
1729                 M[1] = 0;\r
1730                 M[2] = 0;\r
1731                 M[3] = 0;\r
1732 \r
1733                 M[4] = 0;\r
1734                 M[5] = (T)(2/heightOfViewVolume);\r
1735                 M[6] = 0;\r
1736                 M[7] = 0;\r
1737 \r
1738                 M[8] = 0;\r
1739                 M[9] = 0;\r
1740                 // M[10]\r
1741                 M[11] = 0;\r
1742 \r
1743                 M[12] = 0;\r
1744                 M[13] = 0;\r
1745                 // M[14]\r
1746                 M[15] = 1;\r
1747 \r
1748                 if ( zClipFromZero )\r
1749                 {\r
1750                         M[10] = (T)(1/(zNear-zFar));\r
1751                         M[14] = (T)(zNear/(zNear-zFar));\r
1752                 }\r
1753                 else\r
1754                 {\r
1755                         M[10] = (T)(2/(zNear-zFar));\r
1756                         M[14] = (T)-(zFar+zNear)/(zFar-zNear);\r
1757                 }\r
1758 \r
1759 #if defined ( USE_MATRIX_TEST )\r
1760                 definitelyIdentityMatrix=false;\r
1761 #endif\r
1762                 return *this;\r
1763         }\r
1764 \r
1765 \r
1766         // Builds a right-handed perspective projection matrix.\r
1767         template <class T>\r
1768         inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveRH(\r
1769                         f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero)\r
1770         {\r
1771                 _IRR_DEBUG_BREAK_IF(widthOfViewVolume==0.f); //divide by zero\r
1772                 _IRR_DEBUG_BREAK_IF(heightOfViewVolume==0.f); //divide by zero\r
1773                 _IRR_DEBUG_BREAK_IF(zNear==zFar); //divide by zero\r
1774                 M[0] = (T)(2*zNear/widthOfViewVolume);\r
1775                 M[1] = 0;\r
1776                 M[2] = 0;\r
1777                 M[3] = 0;\r
1778 \r
1779                 M[4] = 0;\r
1780                 M[5] = (T)(2*zNear/heightOfViewVolume);\r
1781                 M[6] = 0;\r
1782                 M[7] = 0;\r
1783 \r
1784                 M[8] = 0;\r
1785                 M[9] = 0;\r
1786                 //M[10]\r
1787                 M[11] = -1;\r
1788 \r
1789                 M[12] = 0;\r
1790                 M[13] = 0;\r
1791                 //M[14]\r
1792                 M[15] = 0;\r
1793 \r
1794                 if ( zClipFromZero ) // DirectX version\r
1795                 {\r
1796                         M[10] = (T)(zFar/(zNear-zFar));\r
1797                         M[14] = (T)(zNear*zFar/(zNear-zFar));\r
1798                 }\r
1799                 else    // OpenGL version\r
1800                 {\r
1801                         M[10] = (T)((zFar+zNear)/(zNear-zFar));\r
1802                         M[14] = (T)(2.0f*zNear*zFar/(zNear-zFar));\r
1803                 }\r
1804 \r
1805 #if defined ( USE_MATRIX_TEST )\r
1806                 definitelyIdentityMatrix=false;\r
1807 #endif\r
1808                 return *this;\r
1809         }\r
1810 \r
1811 \r
1812         // Builds a left-handed perspective projection matrix.\r
1813         template <class T>\r
1814         inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveLH(\r
1815                         f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero)\r
1816         {\r
1817                 _IRR_DEBUG_BREAK_IF(widthOfViewVolume==0.f); //divide by zero\r
1818                 _IRR_DEBUG_BREAK_IF(heightOfViewVolume==0.f); //divide by zero\r
1819                 _IRR_DEBUG_BREAK_IF(zNear==zFar); //divide by zero\r
1820                 M[0] = (T)(2*zNear/widthOfViewVolume);\r
1821                 M[1] = 0;\r
1822                 M[2] = 0;\r
1823                 M[3] = 0;\r
1824 \r
1825                 M[4] = 0;\r
1826                 M[5] = (T)(2*zNear/heightOfViewVolume);\r
1827                 M[6] = 0;\r
1828                 M[7] = 0;\r
1829 \r
1830                 M[8] = 0;\r
1831                 M[9] = 0;\r
1832                 //M[10]\r
1833                 M[11] = 1;\r
1834 \r
1835                 M[12] = 0;\r
1836                 M[13] = 0;\r
1837                 //M[14] = (T)(zNear*zFar/(zNear-zFar));\r
1838                 M[15] = 0;\r
1839 \r
1840                 if ( zClipFromZero ) // DirectX version\r
1841                 {\r
1842                         M[10] = (T)(zFar/(zFar-zNear));\r
1843                         M[14] = (T)(zNear*zFar/(zNear-zFar));\r
1844                 }\r
1845                 else    // OpenGL version\r
1846                 {\r
1847                         M[10] = (T)((zFar+zNear)/(zFar-zNear));\r
1848                         M[14] = (T)(2.0f*zNear*zFar/(zNear-zFar));\r
1849                 }\r
1850 \r
1851 #if defined ( USE_MATRIX_TEST )\r
1852                 definitelyIdentityMatrix=false;\r
1853 #endif\r
1854                 return *this;\r
1855         }\r
1856 \r
1857 \r
1858         // Builds a matrix that flattens geometry into a plane.\r
1859         template <class T>\r
1860         inline CMatrix4<T>& CMatrix4<T>::buildShadowMatrix(const core::vector3df& light, core::plane3df plane, f32 point)\r
1861         {\r
1862                 plane.Normal.normalize();\r
1863                 const f32 d = plane.Normal.dotProduct(light);\r
1864 \r
1865                 M[ 0] = (T)(-plane.Normal.X * light.X + d);\r
1866                 M[ 1] = (T)(-plane.Normal.X * light.Y);\r
1867                 M[ 2] = (T)(-plane.Normal.X * light.Z);\r
1868                 M[ 3] = (T)(-plane.Normal.X * point);\r
1869 \r
1870                 M[ 4] = (T)(-plane.Normal.Y * light.X);\r
1871                 M[ 5] = (T)(-plane.Normal.Y * light.Y + d);\r
1872                 M[ 6] = (T)(-plane.Normal.Y * light.Z);\r
1873                 M[ 7] = (T)(-plane.Normal.Y * point);\r
1874 \r
1875                 M[ 8] = (T)(-plane.Normal.Z * light.X);\r
1876                 M[ 9] = (T)(-plane.Normal.Z * light.Y);\r
1877                 M[10] = (T)(-plane.Normal.Z * light.Z + d);\r
1878                 M[11] = (T)(-plane.Normal.Z * point);\r
1879 \r
1880                 M[12] = (T)(-plane.D * light.X);\r
1881                 M[13] = (T)(-plane.D * light.Y);\r
1882                 M[14] = (T)(-plane.D * light.Z);\r
1883                 M[15] = (T)(-plane.D * point + d);\r
1884 #if defined ( USE_MATRIX_TEST )\r
1885                 definitelyIdentityMatrix=false;\r
1886 #endif\r
1887                 return *this;\r
1888         }\r
1889 \r
1890         // Builds a left-handed look-at matrix.\r
1891         template <class T>\r
1892         inline CMatrix4<T>& CMatrix4<T>::buildCameraLookAtMatrixLH(\r
1893                                 const vector3df& position,\r
1894                                 const vector3df& target,\r
1895                                 const vector3df& upVector)\r
1896         {\r
1897                 vector3df zaxis = target - position;\r
1898                 zaxis.normalize();\r
1899 \r
1900                 vector3df xaxis = upVector.crossProduct(zaxis);\r
1901                 xaxis.normalize();\r
1902 \r
1903                 vector3df yaxis = zaxis.crossProduct(xaxis);\r
1904 \r
1905                 M[0] = (T)xaxis.X;\r
1906                 M[1] = (T)yaxis.X;\r
1907                 M[2] = (T)zaxis.X;\r
1908                 M[3] = 0;\r
1909 \r
1910                 M[4] = (T)xaxis.Y;\r
1911                 M[5] = (T)yaxis.Y;\r
1912                 M[6] = (T)zaxis.Y;\r
1913                 M[7] = 0;\r
1914 \r
1915                 M[8] = (T)xaxis.Z;\r
1916                 M[9] = (T)yaxis.Z;\r
1917                 M[10] = (T)zaxis.Z;\r
1918                 M[11] = 0;\r
1919 \r
1920                 M[12] = (T)-xaxis.dotProduct(position);\r
1921                 M[13] = (T)-yaxis.dotProduct(position);\r
1922                 M[14] = (T)-zaxis.dotProduct(position);\r
1923                 M[15] = 1;\r
1924 #if defined ( USE_MATRIX_TEST )\r
1925                 definitelyIdentityMatrix=false;\r
1926 #endif\r
1927                 return *this;\r
1928         }\r
1929 \r
1930 \r
1931         // Builds a right-handed look-at matrix.\r
1932         template <class T>\r
1933         inline CMatrix4<T>& CMatrix4<T>::buildCameraLookAtMatrixRH(\r
1934                                 const vector3df& position,\r
1935                                 const vector3df& target,\r
1936                                 const vector3df& upVector)\r
1937         {\r
1938                 vector3df zaxis = position - target;\r
1939                 zaxis.normalize();\r
1940 \r
1941                 vector3df xaxis = upVector.crossProduct(zaxis);\r
1942                 xaxis.normalize();\r
1943 \r
1944                 vector3df yaxis = zaxis.crossProduct(xaxis);\r
1945 \r
1946                 M[0] = (T)xaxis.X;\r
1947                 M[1] = (T)yaxis.X;\r
1948                 M[2] = (T)zaxis.X;\r
1949                 M[3] = 0;\r
1950 \r
1951                 M[4] = (T)xaxis.Y;\r
1952                 M[5] = (T)yaxis.Y;\r
1953                 M[6] = (T)zaxis.Y;\r
1954                 M[7] = 0;\r
1955 \r
1956                 M[8] = (T)xaxis.Z;\r
1957                 M[9] = (T)yaxis.Z;\r
1958                 M[10] = (T)zaxis.Z;\r
1959                 M[11] = 0;\r
1960 \r
1961                 M[12] = (T)-xaxis.dotProduct(position);\r
1962                 M[13] = (T)-yaxis.dotProduct(position);\r
1963                 M[14] = (T)-zaxis.dotProduct(position);\r
1964                 M[15] = 1;\r
1965 #if defined ( USE_MATRIX_TEST )\r
1966                 definitelyIdentityMatrix=false;\r
1967 #endif\r
1968                 return *this;\r
1969         }\r
1970 \r
1971 \r
1972         // creates a new matrix as interpolated matrix from this and the passed one.\r
1973         template <class T>\r
1974         inline CMatrix4<T> CMatrix4<T>::interpolate(const core::CMatrix4<T>& b, f32 time) const\r
1975         {\r
1976                 CMatrix4<T> mat ( EM4CONST_NOTHING );\r
1977 \r
1978                 for (u32 i=0; i < 16; i += 4)\r
1979                 {\r
1980                         mat.M[i+0] = (T)(M[i+0] + ( b.M[i+0] - M[i+0] ) * time);\r
1981                         mat.M[i+1] = (T)(M[i+1] + ( b.M[i+1] - M[i+1] ) * time);\r
1982                         mat.M[i+2] = (T)(M[i+2] + ( b.M[i+2] - M[i+2] ) * time);\r
1983                         mat.M[i+3] = (T)(M[i+3] + ( b.M[i+3] - M[i+3] ) * time);\r
1984                 }\r
1985                 return mat;\r
1986         }\r
1987 \r
1988 \r
1989         // returns transposed matrix\r
1990         template <class T>\r
1991         inline CMatrix4<T> CMatrix4<T>::getTransposed() const\r
1992         {\r
1993                 CMatrix4<T> t ( EM4CONST_NOTHING );\r
1994                 getTransposed ( t );\r
1995                 return t;\r
1996         }\r
1997 \r
1998 \r
1999         // returns transposed matrix\r
2000         template <class T>\r
2001         inline void CMatrix4<T>::getTransposed( CMatrix4<T>& o ) const\r
2002         {\r
2003                 o[ 0] = M[ 0];\r
2004                 o[ 1] = M[ 4];\r
2005                 o[ 2] = M[ 8];\r
2006                 o[ 3] = M[12];\r
2007 \r
2008                 o[ 4] = M[ 1];\r
2009                 o[ 5] = M[ 5];\r
2010                 o[ 6] = M[ 9];\r
2011                 o[ 7] = M[13];\r
2012 \r
2013                 o[ 8] = M[ 2];\r
2014                 o[ 9] = M[ 6];\r
2015                 o[10] = M[10];\r
2016                 o[11] = M[14];\r
2017 \r
2018                 o[12] = M[ 3];\r
2019                 o[13] = M[ 7];\r
2020                 o[14] = M[11];\r
2021                 o[15] = M[15];\r
2022 #if defined ( USE_MATRIX_TEST )\r
2023                 o.definitelyIdentityMatrix=definitelyIdentityMatrix;\r
2024 #endif\r
2025         }\r
2026 \r
2027 \r
2028         // used to scale <-1,-1><1,1> to viewport\r
2029         template <class T>\r
2030         inline CMatrix4<T>& CMatrix4<T>::buildNDCToDCMatrix( const core::rect<s32>& viewport, f32 zScale)\r
2031         {\r
2032                 const f32 scaleX = (viewport.getWidth() - 0.75f ) * 0.5f;\r
2033                 const f32 scaleY = -(viewport.getHeight() - 0.75f ) * 0.5f;\r
2034 \r
2035                 const f32 dx = -0.5f + ( (viewport.UpperLeftCorner.X + viewport.LowerRightCorner.X ) * 0.5f );\r
2036                 const f32 dy = -0.5f + ( (viewport.UpperLeftCorner.Y + viewport.LowerRightCorner.Y ) * 0.5f );\r
2037 \r
2038                 makeIdentity();\r
2039                 M[12] = (T)dx;\r
2040                 M[13] = (T)dy;\r
2041                 return setScale(core::vector3d<T>((T)scaleX, (T)scaleY, (T)zScale));\r
2042         }\r
2043 \r
2044         //! Builds a matrix that rotates from one vector to another\r
2045         /** \param from: vector to rotate from\r
2046         \param to: vector to rotate to\r
2047 \r
2048                 http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm\r
2049          */\r
2050         template <class T>\r
2051         inline CMatrix4<T>& CMatrix4<T>::buildRotateFromTo(const core::vector3df& from, const core::vector3df& to)\r
2052         {\r
2053                 // unit vectors\r
2054                 core::vector3df f(from);\r
2055                 core::vector3df t(to);\r
2056                 f.normalize();\r
2057                 t.normalize();\r
2058 \r
2059                 // axis multiplication by sin\r
2060                 core::vector3df vs(t.crossProduct(f));\r
2061 \r
2062                 // axis of rotation\r
2063                 core::vector3df v(vs);\r
2064                 v.normalize();\r
2065 \r
2066                 // cosinus angle\r
2067                 T ca = f.dotProduct(t);\r
2068 \r
2069                 core::vector3df vt(v * (1 - ca));\r
2070 \r
2071                 M[0] = vt.X * v.X + ca;\r
2072                 M[5] = vt.Y * v.Y + ca;\r
2073                 M[10] = vt.Z * v.Z + ca;\r
2074 \r
2075                 vt.X *= v.Y;\r
2076                 vt.Z *= v.X;\r
2077                 vt.Y *= v.Z;\r
2078 \r
2079                 M[1] = vt.X - vs.Z;\r
2080                 M[2] = vt.Z + vs.Y;\r
2081                 M[3] = 0;\r
2082 \r
2083                 M[4] = vt.X + vs.Z;\r
2084                 M[6] = vt.Y - vs.X;\r
2085                 M[7] = 0;\r
2086 \r
2087                 M[8] = vt.Z - vs.Y;\r
2088                 M[9] = vt.Y + vs.X;\r
2089                 M[11] = 0;\r
2090 \r
2091                 M[12] = 0;\r
2092                 M[13] = 0;\r
2093                 M[14] = 0;\r
2094                 M[15] = 1;\r
2095 \r
2096                 return *this;\r
2097         }\r
2098 \r
2099         //! Builds a matrix which rotates a source vector to a look vector over an arbitrary axis\r
2100         /** \param camPos: viewer position in world coord\r
2101         \param center: object position in world-coord, rotation pivot\r
2102         \param translation: object final translation from center\r
2103         \param axis: axis to rotate about\r
2104         \param from: source vector to rotate from\r
2105          */\r
2106         template <class T>\r
2107         inline void CMatrix4<T>::buildAxisAlignedBillboard(\r
2108                                 const core::vector3df& camPos,\r
2109                                 const core::vector3df& center,\r
2110                                 const core::vector3df& translation,\r
2111                                 const core::vector3df& axis,\r
2112                                 const core::vector3df& from)\r
2113         {\r
2114                 // axis of rotation\r
2115                 core::vector3df up = axis;\r
2116                 up.normalize();\r
2117                 const core::vector3df forward = (camPos - center).normalize();\r
2118                 const core::vector3df right = up.crossProduct(forward).normalize();\r
2119 \r
2120                 // correct look vector\r
2121                 const core::vector3df look = right.crossProduct(up);\r
2122 \r
2123                 // rotate from to\r
2124                 // axis multiplication by sin\r
2125                 const core::vector3df vs = look.crossProduct(from);\r
2126 \r
2127                 // cosinus angle\r
2128                 const f32 ca = from.dotProduct(look);\r
2129 \r
2130                 core::vector3df vt(up * (1.f - ca));\r
2131 \r
2132                 M[0] = static_cast<T>(vt.X * up.X + ca);\r
2133                 M[5] = static_cast<T>(vt.Y * up.Y + ca);\r
2134                 M[10] = static_cast<T>(vt.Z * up.Z + ca);\r
2135 \r
2136                 vt.X *= up.Y;\r
2137                 vt.Z *= up.X;\r
2138                 vt.Y *= up.Z;\r
2139 \r
2140                 M[1] = static_cast<T>(vt.X - vs.Z);\r
2141                 M[2] = static_cast<T>(vt.Z + vs.Y);\r
2142                 M[3] = 0;\r
2143 \r
2144                 M[4] = static_cast<T>(vt.X + vs.Z);\r
2145                 M[6] = static_cast<T>(vt.Y - vs.X);\r
2146                 M[7] = 0;\r
2147 \r
2148                 M[8] = static_cast<T>(vt.Z - vs.Y);\r
2149                 M[9] = static_cast<T>(vt.Y + vs.X);\r
2150                 M[11] = 0;\r
2151 \r
2152                 setRotationCenter(center, translation);\r
2153         }\r
2154 \r
2155 \r
2156         //! Builds a combined matrix which translate to a center before rotation and translate afterward\r
2157         template <class T>\r
2158         inline void CMatrix4<T>::setRotationCenter(const core::vector3df& center, const core::vector3df& translation)\r
2159         {\r
2160                 M[12] = -M[0]*center.X - M[4]*center.Y - M[8]*center.Z + (center.X - translation.X );\r
2161                 M[13] = -M[1]*center.X - M[5]*center.Y - M[9]*center.Z + (center.Y - translation.Y );\r
2162                 M[14] = -M[2]*center.X - M[6]*center.Y - M[10]*center.Z + (center.Z - translation.Z );\r
2163                 M[15] = (T) 1.0;\r
2164 #if defined ( USE_MATRIX_TEST )\r
2165                 definitelyIdentityMatrix=false;\r
2166 #endif\r
2167         }\r
2168 \r
2169         /*!\r
2170                 Generate texture coordinates as linear functions so that:\r
2171                         u = Ux*x + Uy*y + Uz*z + Uw\r
2172                         v = Vx*x + Vy*y + Vz*z + Vw\r
2173                 The matrix M for this case is:\r
2174                         Ux  Vx  0  0\r
2175                         Uy  Vy  0  0\r
2176                         Uz  Vz  0  0\r
2177                         Uw  Vw  0  0\r
2178         */\r
2179 \r
2180 \r
2181         template <class T>\r
2182         inline CMatrix4<T>& CMatrix4<T>::buildTextureTransform( f32 rotateRad,\r
2183                         const core::vector2df &rotatecenter,\r
2184                         const core::vector2df &translate,\r
2185                         const core::vector2df &scale)\r
2186         {\r
2187                 const f32 c = cosf(rotateRad);\r
2188                 const f32 s = sinf(rotateRad);\r
2189 \r
2190                 M[0] = (T)(c * scale.X);\r
2191                 M[1] = (T)(s * scale.Y);\r
2192                 M[2] = 0;\r
2193                 M[3] = 0;\r
2194 \r
2195                 M[4] = (T)(-s * scale.X);\r
2196                 M[5] = (T)(c * scale.Y);\r
2197                 M[6] = 0;\r
2198                 M[7] = 0;\r
2199 \r
2200                 M[8] = (T)(c * scale.X * rotatecenter.X + -s * rotatecenter.Y + translate.X);\r
2201                 M[9] = (T)(s * scale.Y * rotatecenter.X +  c * rotatecenter.Y + translate.Y);\r
2202                 M[10] = 1;\r
2203                 M[11] = 0;\r
2204 \r
2205                 M[12] = 0;\r
2206                 M[13] = 0;\r
2207                 M[14] = 0;\r
2208                 M[15] = 1;\r
2209 #if defined ( USE_MATRIX_TEST )\r
2210                 definitelyIdentityMatrix=false;\r
2211 #endif\r
2212                 return *this;\r
2213         }\r
2214 \r
2215 \r
2216         // rotate about z axis, center ( 0.5, 0.5 )\r
2217         template <class T>\r
2218         inline CMatrix4<T>& CMatrix4<T>::setTextureRotationCenter( f32 rotateRad )\r
2219         {\r
2220                 const f32 c = cosf(rotateRad);\r
2221                 const f32 s = sinf(rotateRad);\r
2222                 M[0] = (T)c;\r
2223                 M[1] = (T)s;\r
2224 \r
2225                 M[4] = (T)-s;\r
2226                 M[5] = (T)c;\r
2227 \r
2228                 M[8] = (T)(0.5f * ( s - c) + 0.5f);\r
2229                 M[9] = (T)(-0.5f * ( s + c) + 0.5f);\r
2230 \r
2231 #if defined ( USE_MATRIX_TEST )\r
2232                 definitelyIdentityMatrix = definitelyIdentityMatrix && (rotateRad==0.0f);\r
2233 #endif\r
2234                 return *this;\r
2235         }\r
2236 \r
2237 \r
2238         template <class T>\r
2239         inline CMatrix4<T>& CMatrix4<T>::setTextureTranslate ( f32 x, f32 y )\r
2240         {\r
2241                 M[8] = (T)x;\r
2242                 M[9] = (T)y;\r
2243 \r
2244 #if defined ( USE_MATRIX_TEST )\r
2245                 definitelyIdentityMatrix = definitelyIdentityMatrix && (x==0.0f) && (y==0.0f);\r
2246 #endif\r
2247                 return *this;\r
2248         }\r
2249 \r
2250         template <class T>\r
2251         inline void CMatrix4<T>::getTextureTranslate(f32& x, f32& y) const\r
2252         {\r
2253                 x = (f32)M[8];\r
2254                 y = (f32)M[9];\r
2255         }\r
2256 \r
2257         template <class T>\r
2258         inline CMatrix4<T>& CMatrix4<T>::setTextureTranslateTransposed ( f32 x, f32 y )\r
2259         {\r
2260                 M[2] = (T)x;\r
2261                 M[6] = (T)y;\r
2262 \r
2263 #if defined ( USE_MATRIX_TEST )\r
2264                 definitelyIdentityMatrix = definitelyIdentityMatrix && (x==0.0f) && (y==0.0f);\r
2265 #endif\r
2266                 return *this;\r
2267         }\r
2268 \r
2269         template <class T>\r
2270         inline CMatrix4<T>& CMatrix4<T>::setTextureScale ( f32 sx, f32 sy )\r
2271         {\r
2272                 M[0] = (T)sx;\r
2273                 M[5] = (T)sy;\r
2274 #if defined ( USE_MATRIX_TEST )\r
2275                 definitelyIdentityMatrix = definitelyIdentityMatrix && (sx==1.0f) && (sy==1.0f);\r
2276 #endif\r
2277                 return *this;\r
2278         }\r
2279 \r
2280         template <class T>\r
2281         inline void CMatrix4<T>::getTextureScale ( f32& sx, f32& sy ) const\r
2282         {\r
2283                 sx = (f32)M[0];\r
2284                 sy = (f32)M[5];\r
2285         }\r
2286 \r
2287         template <class T>\r
2288         inline CMatrix4<T>& CMatrix4<T>::setTextureScaleCenter( f32 sx, f32 sy )\r
2289         {\r
2290                 M[0] = (T)sx;\r
2291                 M[5] = (T)sy;\r
2292                 M[8] = (T)(0.5f - 0.5f * sx);\r
2293                 M[9] = (T)(0.5f - 0.5f * sy);\r
2294 \r
2295 #if defined ( USE_MATRIX_TEST )\r
2296                 definitelyIdentityMatrix = definitelyIdentityMatrix && (sx==1.0f) && (sy==1.0f);\r
2297 #endif\r
2298                 return *this;\r
2299         }\r
2300 \r
2301 \r
2302         // sets all matrix data members at once\r
2303         template <class T>\r
2304         inline CMatrix4<T>& CMatrix4<T>::setM(const T* data)\r
2305         {\r
2306                 memcpy(M,data, 16*sizeof(T));\r
2307 \r
2308 #if defined ( USE_MATRIX_TEST )\r
2309                 definitelyIdentityMatrix=false;\r
2310 #endif\r
2311                 return *this;\r
2312         }\r
2313 \r
2314 \r
2315         // sets if the matrix is definitely identity matrix\r
2316         template <class T>\r
2317         inline void CMatrix4<T>::setDefinitelyIdentityMatrix( bool isDefinitelyIdentityMatrix)\r
2318         {\r
2319 #if defined ( USE_MATRIX_TEST )\r
2320                 definitelyIdentityMatrix = isDefinitelyIdentityMatrix;\r
2321 #else\r
2322                 (void)isDefinitelyIdentityMatrix; // prevent compiler warning\r
2323 #endif\r
2324         }\r
2325 \r
2326 \r
2327         // gets if the matrix is definitely identity matrix\r
2328         template <class T>\r
2329         inline bool CMatrix4<T>::getDefinitelyIdentityMatrix() const\r
2330         {\r
2331 #if defined ( USE_MATRIX_TEST )\r
2332                 return definitelyIdentityMatrix;\r
2333 #else\r
2334                 return false;\r
2335 #endif\r
2336         }\r
2337 \r
2338 \r
2339         //! Compare two matrices using the equal method\r
2340         template <class T>\r
2341         inline bool CMatrix4<T>::equals(const core::CMatrix4<T>& other, const T tolerance) const\r
2342         {\r
2343 #if defined ( USE_MATRIX_TEST )\r
2344                 if (definitelyIdentityMatrix && other.definitelyIdentityMatrix)\r
2345                         return true;\r
2346 #endif\r
2347                 for (s32 i = 0; i < 16; ++i)\r
2348                         if (!core::equals(M[i],other.M[i], tolerance))\r
2349                                 return false;\r
2350 \r
2351                 return true;\r
2352         }\r
2353 \r
2354 \r
2355         // Multiply by scalar.\r
2356         template <class T>\r
2357         inline CMatrix4<T> operator*(const T scalar, const CMatrix4<T>& mat)\r
2358         {\r
2359                 return mat*scalar;\r
2360         }\r
2361 \r
2362 \r
2363         //! Typedef for f32 matrix\r
2364         typedef CMatrix4<f32> matrix4;\r
2365 \r
2366         //! global const identity matrix\r
2367         IRRLICHT_API extern const matrix4 IdentityMatrix;\r
2368 \r
2369 } // end namespace core\r
2370 } // end namespace irr\r
2371 \r
2372 #endif\r
2373 \r