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