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