Extras/CDTestFramework/Opcode/Ice/IceMatrix3x3.h

   1 /*
   2  *      ICE / OPCODE - Optimized Collision Detection
   3  * http://www.codercorner.com/Opcode.htm
   4  *
   5  * Copyright (c) 2001-2008 Pierre Terdiman,  pierre@codercorner.com
   6
   7 This software is provided 'as-is', without any express or implied warranty.
   8 In no event will the authors be held liable for any damages arising from the use of this software.
   9 Permission is granted to anyone to use this software for any purpose,
  10 including commercial applications, and to alter it and redistribute it freely,
  11 subject to the following restrictions:
  12
  13 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  14 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  15 3. This notice may not be removed or altered from any source distribution.
  16 */
  17 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  18 /**
  19  *      Contains code for 3x3 matrices.
  20  *      \file           IceMatrix3x3.h
  21  *      \author         Pierre Terdiman
  22  *      \date           April, 4, 2000
  23  */
  24 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  25
  26 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  27 // Include Guard
  28 #ifndef __ICEMATRIX3X3_H__
  29 #define __ICEMATRIX3X3_H__
  30
  31         // Forward declarations
  32         class Quat;
  33
  34         #define MATRIX3X3_EPSILON               (1.0e-7f)
  35
  36         class ICEMATHS_API Matrix3x3
  37         {
  38                 public:
  39                 //! Empty constructor
  40                 inline_                                 Matrix3x3()                                                                     {}
  41                 //! Constructor from 9 values
  42                 inline_                                 Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
  43                                                                 {
  44                                                                         m[0][0] = m00;  m[0][1] = m01;  m[0][2] = m02;
  45                                                                         m[1][0] = m10;  m[1][1] = m11;  m[1][2] = m12;
  46                                                                         m[2][0] = m20;  m[2][1] = m21;  m[2][2] = m22;
  47                                                                 }
  48                 //! Copy constructor
  49                 inline_                                 Matrix3x3(const Matrix3x3& mat)                         { CopyMemory(m, &mat.m, 9*sizeof(float));       }
  50                 //! Destructor
  51                 inline_                                 ~Matrix3x3()                                                            {}
  52
  53                 //! Assign values
  54                 inline_ void                    Set(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
  55                                                                 {
  56                                                                         m[0][0] = m00;  m[0][1] = m01;  m[0][2] = m02;
  57                                                                         m[1][0] = m10;  m[1][1] = m11;  m[1][2] = m12;
  58                                                                         m[2][0] = m20;  m[2][1] = m21;  m[2][2] = m22;
  59                                                                 }
  60
  61                 //! Sets the scale from a Point. The point is put on the diagonal.
  62                 inline_ void                    SetScale(const Point& p)                                        { m[0][0] = p.x;        m[1][1] = p.y;  m[2][2] = p.z;  }
  63
  64                 //! Sets the scale from floats. Values are put on the diagonal.
  65                 inline_ void                    SetScale(float sx, float sy, float sz)          { m[0][0] = sx;         m[1][1] = sy;   m[2][2] = sz;   }
  66
  67                 //! Scales from a Point. Each row is multiplied by a component.
  68                 inline_ void                    Scale(const Point& p)
  69                                                                 {
  70                                                                         m[0][0] *= p.x; m[0][1] *= p.x; m[0][2] *= p.x;
  71                                                                         m[1][0] *= p.y; m[1][1] *= p.y; m[1][2] *= p.y;
  72                                                                         m[2][0] *= p.z; m[2][1] *= p.z; m[2][2] *= p.z;
  73                                                                 }
  74
  75                 //! Scales from floats. Each row is multiplied by a value.
  76                 inline_ void                    Scale(float sx, float sy, float sz)
  77                                                                 {
  78                                                                         m[0][0] *= sx;  m[0][1] *= sx;  m[0][2] *= sx;
  79                                                                         m[1][0] *= sy;  m[1][1] *= sy;  m[1][2] *= sy;
  80                                                                         m[2][0] *= sz;  m[2][1] *= sz;  m[2][2] *= sz;
  81                                                                 }
  82
  83                 //! Copy from a Matrix3x3
  84                 inline_ void                    Copy(const Matrix3x3& source)                           { CopyMemory(m, source.m, 9*sizeof(float));                     }
  85
  86                 // Row-column access
  87                 //! Returns a row.
  88                 inline_ void                    GetRow(const udword r, Point& p)        const   { p.x = m[r][0];        p.y = m[r][1];  p.z = m[r][2];  }
  89                 //! Returns a row.
  90                 inline_ const Point&    GetRow(const udword r)                          const   { return *(const Point*)&m[r][0];       }
  91                 //! Returns a row.
  92                 inline_ Point&                  GetRow(const udword r)                                          { return *(Point*)&m[r][0];                     }
  93                 //! Sets a row.
  94                 inline_ void                    SetRow(const udword r, const Point& p)          { m[r][0] = p.x;        m[r][1] = p.y;  m[r][2] = p.z;  }
  95                 //! Returns a column.
  96                 inline_ void                    GetCol(const udword c, Point& p)        const   { p.x = m[0][c];        p.y = m[1][c];  p.z = m[2][c];  }
  97                 //! Sets a column.
  98                 inline_ void                    SetCol(const udword c, const Point& p)          { m[0][c] = p.x;        m[1][c] = p.y;  m[2][c] = p.z;  }
  99
 100                 //! Computes the trace. The trace is the sum of the 3 diagonal components.
 101                 inline_ float                   Trace()                                 const                           { return m[0][0] + m[1][1] + m[2][2];                           }
 102                 //! Clears the matrix.
 103                 inline_ void                    Zero()                                                                          { ZeroMemory(&m, sizeof(m));                                            }
 104                 //! Sets the identity matrix.
 105                 inline_ void                    Identity()                                                                      { Zero(); m[0][0] = m[1][1] = m[2][2] = 1.0f;           }
 106                 //! Checks for identity
 107                 inline_ bool                    IsIdentity()                    const
 108                                                                 {
 109                                                                         if(IR(m[0][0])!=IEEE_1_0)       return false;
 110                                                                         if(IR(m[0][1])!=0)                      return false;
 111                                                                         if(IR(m[0][2])!=0)                      return false;
 112
 113                                                                         if(IR(m[1][0])!=0)                      return false;
 114                                                                         if(IR(m[1][1])!=IEEE_1_0)       return false;
 115                                                                         if(IR(m[1][2])!=0)                      return false;
 116
 117                                                                         if(IR(m[2][0])!=0)                      return false;
 118                                                                         if(IR(m[2][1])!=0)                      return false;
 119                                                                         if(IR(m[2][2])!=IEEE_1_0)       return false;
 120
 121                                                                         return true;
 122                                                                 }
 123
 124                 //! Checks matrix validity
 125                 inline_ BOOL                    IsValid()                               const
 126                                                                 {
 127                                                                         for(udword j=0;j<3;j++)
 128                                                                         {
 129                                                                                 for(udword i=0;i<3;i++)
 130                                                                                 {
 131                                                                                         if(!IsValidFloat(m[j][i]))      return FALSE;
 132                                                                                 }
 133                                                                         }
 134                                                                         return TRUE;
 135                                                                 }
 136
 137                 //! Makes a skew-symmetric matrix (a.k.a. Star(*) Matrix)
 138                 //!     [  0.0  -a.z   a.y ]
 139                 //!     [  a.z   0.0  -a.x ]
 140                 //!     [ -a.y   a.x   0.0 ]
 141                 //! This is also called a "cross matrix" since for any vectors A and B,
 142                 //! A^B = Skew(A) * B = - B * Skew(A);
 143                 inline_ void                    SkewSymmetric(const Point& a)
 144                                                                 {
 145                                                                         m[0][0] = 0.0f;
 146                                                                         m[0][1] = -a.z;
 147                                                                         m[0][2] = a.y;
 148
 149                                                                         m[1][0] = a.z;
 150                                                                         m[1][1] = 0.0f;
 151                                                                         m[1][2] = -a.x;
 152
 153                                                                         m[2][0] = -a.y;
 154                                                                         m[2][1] = a.x;
 155                                                                         m[2][2] = 0.0f;
 156                                                                 }
 157
 158                 //! Negates the matrix
 159                 inline_ void                    Neg()
 160                                                                 {
 161                                                                         m[0][0] = -m[0][0];     m[0][1] = -m[0][1];     m[0][2] = -m[0][2];
 162                                                                         m[1][0] = -m[1][0];     m[1][1] = -m[1][1];     m[1][2] = -m[1][2];
 163                                                                         m[2][0] = -m[2][0];     m[2][1] = -m[2][1];     m[2][2] = -m[2][2];
 164                                                                 }
 165
 166                 //! Neg from another matrix
 167                 inline_ void                    Neg(const Matrix3x3& mat)
 168                                                                 {
 169                                                                         m[0][0] = -mat.m[0][0]; m[0][1] = -mat.m[0][1]; m[0][2] = -mat.m[0][2];
 170                                                                         m[1][0] = -mat.m[1][0]; m[1][1] = -mat.m[1][1]; m[1][2] = -mat.m[1][2];
 171                                                                         m[2][0] = -mat.m[2][0]; m[2][1] = -mat.m[2][1]; m[2][2] = -mat.m[2][2];
 172                                                                 }
 173
 174                 //! Add another matrix
 175                 inline_ void                    Add(const Matrix3x3& mat)
 176                                                                 {
 177                                                                         m[0][0] += mat.m[0][0]; m[0][1] += mat.m[0][1]; m[0][2] += mat.m[0][2];
 178                                                                         m[1][0] += mat.m[1][0]; m[1][1] += mat.m[1][1]; m[1][2] += mat.m[1][2];
 179                                                                         m[2][0] += mat.m[2][0]; m[2][1] += mat.m[2][1]; m[2][2] += mat.m[2][2];
 180                                                                 }
 181
 182                 //! Sub another matrix
 183                 inline_ void                    Sub(const Matrix3x3& mat)
 184                                                                 {
 185                                                                         m[0][0] -= mat.m[0][0]; m[0][1] -= mat.m[0][1]; m[0][2] -= mat.m[0][2];
 186                                                                         m[1][0] -= mat.m[1][0]; m[1][1] -= mat.m[1][1]; m[1][2] -= mat.m[1][2];
 187                                                                         m[2][0] -= mat.m[2][0]; m[2][1] -= mat.m[2][1]; m[2][2] -= mat.m[2][2];
 188                                                                 }
 189                 //! Mac
 190                 inline_ void                    Mac(const Matrix3x3& a, const Matrix3x3& b, float s)
 191                                                                 {
 192                                                                         m[0][0] = a.m[0][0] + b.m[0][0] * s;
 193                                                                         m[0][1] = a.m[0][1] + b.m[0][1] * s;
 194                                                                         m[0][2] = a.m[0][2] + b.m[0][2] * s;
 195
 196                                                                         m[1][0] = a.m[1][0] + b.m[1][0] * s;
 197                                                                         m[1][1] = a.m[1][1] + b.m[1][1] * s;
 198                                                                         m[1][2] = a.m[1][2] + b.m[1][2] * s;
 199
 200                                                                         m[2][0] = a.m[2][0] + b.m[2][0] * s;
 201                                                                         m[2][1] = a.m[2][1] + b.m[2][1] * s;
 202                                                                         m[2][2] = a.m[2][2] + b.m[2][2] * s;
 203                                                                 }
 204                 //! Mac
 205                 inline_ void                    Mac(const Matrix3x3& a, float s)
 206                                                                 {
 207                                                                         m[0][0] += a.m[0][0] * s;       m[0][1] += a.m[0][1] * s;       m[0][2] += a.m[0][2] * s;
 208                                                                         m[1][0] += a.m[1][0] * s;       m[1][1] += a.m[1][1] * s;       m[1][2] += a.m[1][2] * s;
 209                                                                         m[2][0] += a.m[2][0] * s;       m[2][1] += a.m[2][1] * s;       m[2][2] += a.m[2][2] * s;
 210                                                                 }
 211
 212                 //! this = A * s
 213                 inline_ void                    Mult(const Matrix3x3& a, float s)
 214                                                                 {
 215                                                                         m[0][0] = a.m[0][0] * s;        m[0][1] = a.m[0][1] * s;        m[0][2] = a.m[0][2] * s;
 216                                                                         m[1][0] = a.m[1][0] * s;        m[1][1] = a.m[1][1] * s;        m[1][2] = a.m[1][2] * s;
 217                                                                         m[2][0] = a.m[2][0] * s;        m[2][1] = a.m[2][1] * s;        m[2][2] = a.m[2][2] * s;
 218                                                                 }
 219
 220                 inline_ void                    Add(const Matrix3x3& a, const Matrix3x3& b)
 221                                                                 {
 222                                                                         m[0][0] = a.m[0][0] + b.m[0][0];        m[0][1] = a.m[0][1] + b.m[0][1];        m[0][2] = a.m[0][2] + b.m[0][2];
 223                                                                         m[1][0] = a.m[1][0] + b.m[1][0];        m[1][1] = a.m[1][1] + b.m[1][1];        m[1][2] = a.m[1][2] + b.m[1][2];
 224                                                                         m[2][0] = a.m[2][0] + b.m[2][0];        m[2][1] = a.m[2][1] + b.m[2][1];        m[2][2] = a.m[2][2] + b.m[2][2];
 225                                                                 }
 226
 227                 inline_ void                    Sub(const Matrix3x3& a, const Matrix3x3& b)
 228                                                                 {
 229                                                                         m[0][0] = a.m[0][0] - b.m[0][0];        m[0][1] = a.m[0][1] - b.m[0][1];        m[0][2] = a.m[0][2] - b.m[0][2];
 230                                                                         m[1][0] = a.m[1][0] - b.m[1][0];        m[1][1] = a.m[1][1] - b.m[1][1];        m[1][2] = a.m[1][2] - b.m[1][2];
 231                                                                         m[2][0] = a.m[2][0] - b.m[2][0];        m[2][1] = a.m[2][1] - b.m[2][1];        m[2][2] = a.m[2][2] - b.m[2][2];
 232                                                                 }
 233
 234                 //! this = a * b
 235                 inline_ void                    Mult(const Matrix3x3& a, const Matrix3x3& b)
 236                                                                 {
 237                                                                         m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0];
 238                                                                         m[0][1] = a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1];
 239                                                                         m[0][2] = a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2];
 240                                                                         m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0];
 241                                                                         m[1][1] = a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1];
 242                                                                         m[1][2] = a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2];
 243                                                                         m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0];
 244                                                                         m[2][1] = a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1];
 245                                                                         m[2][2] = a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2];
 246                                                                 }
 247
 248                 //! this = transpose(a) * b
 249                 inline_ void                    MultAtB(const Matrix3x3& a, const Matrix3x3& b)
 250                                                                 {
 251                                                                         m[0][0] = a.m[0][0] * b.m[0][0] + a.m[1][0] * b.m[1][0] + a.m[2][0] * b.m[2][0];
 252                                                                         m[0][1] = a.m[0][0] * b.m[0][1] + a.m[1][0] * b.m[1][1] + a.m[2][0] * b.m[2][1];
 253                                                                         m[0][2] = a.m[0][0] * b.m[0][2] + a.m[1][0] * b.m[1][2] + a.m[2][0] * b.m[2][2];
 254                                                                         m[1][0] = a.m[0][1] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[2][1] * b.m[2][0];
 255                                                                         m[1][1] = a.m[0][1] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[2][1] * b.m[2][1];
 256                                                                         m[1][2] = a.m[0][1] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[2][1] * b.m[2][2];
 257                                                                         m[2][0] = a.m[0][2] * b.m[0][0] + a.m[1][2] * b.m[1][0] + a.m[2][2] * b.m[2][0];
 258                                                                         m[2][1] = a.m[0][2] * b.m[0][1] + a.m[1][2] * b.m[1][1] + a.m[2][2] * b.m[2][1];
 259                                                                         m[2][2] = a.m[0][2] * b.m[0][2] + a.m[1][2] * b.m[1][2] + a.m[2][2] * b.m[2][2];
 260                                                                 }
 261
 262                 //! this = a * transpose(b)
 263                 inline_ void                    MultABt(const Matrix3x3& a, const Matrix3x3& b)
 264                                                                 {
 265                                                                         m[0][0] = a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[0][1] + a.m[0][2] * b.m[0][2];
 266                                                                         m[0][1] = a.m[0][0] * b.m[1][0] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[1][2];
 267                                                                         m[0][2] = a.m[0][0] * b.m[2][0] + a.m[0][1] * b.m[2][1] + a.m[0][2] * b.m[2][2];
 268                                                                         m[1][0] = a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[0][1] + a.m[1][2] * b.m[0][2];
 269                                                                         m[1][1] = a.m[1][0] * b.m[1][0] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[1][2];
 270                                                                         m[1][2] = a.m[1][0] * b.m[2][0] + a.m[1][1] * b.m[2][1] + a.m[1][2] * b.m[2][2];
 271                                                                         m[2][0] = a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[0][1] + a.m[2][2] * b.m[0][2];
 272                                                                         m[2][1] = a.m[2][0] * b.m[1][0] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[1][2];
 273                                                                         m[2][2] = a.m[2][0] * b.m[2][0] + a.m[2][1] * b.m[2][1] + a.m[2][2] * b.m[2][2];
 274                                                                 }
 275
 276                 //! Makes a rotation matrix mapping vector "from" to vector "to".
 277                                 Matrix3x3&              FromTo(const Point& from, const Point& to);
 278
 279                 //! Set a rotation matrix around the X axis.
 280                 //!              1              0               0
 281                 //!     RX = 0          cx              sx
 282                 //!              0              -sx             cx
 283                                 void                    RotX(float angle);
 284                 //! Set a rotation matrix around the Y axis.
 285                 //!              cy             0               -sy
 286                 //!     RY = 0          1               0
 287                 //!              sy             0               cy
 288                                 void                    RotY(float angle);
 289                 //! Set a rotation matrix around the Z axis.
 290                 //!              cz             sz              0
 291                 //!     RZ = -sz        cz              0
 292                 //!              0              0               1
 293                                 void                    RotZ(float angle);
 294                 //!                     cy              sx.sy           -sy.cx
 295                 //!     RY.RX   0               cx                      sx
 296                 //!                     sy              -sx.cy          cx.cy
 297                                 void                    RotYX(float y, float x);
 298
 299                 //! Make a rotation matrix about an arbitrary axis
 300                                 Matrix3x3&              Rot(float angle, const Point& axis);
 301
 302                 //! Transpose the matrix.
 303                                 void                    Transpose()
 304                                                                 {
 305                                                                         IR(m[1][0]) ^= IR(m[0][1]);     IR(m[0][1]) ^= IR(m[1][0]);     IR(m[1][0]) ^= IR(m[0][1]);
 306                                                                         IR(m[2][0]) ^= IR(m[0][2]);     IR(m[0][2]) ^= IR(m[2][0]);     IR(m[2][0]) ^= IR(m[0][2]);
 307                                                                         IR(m[2][1]) ^= IR(m[1][2]);     IR(m[1][2]) ^= IR(m[2][1]);     IR(m[2][1]) ^= IR(m[1][2]);
 308                                                                 }
 309
 310                 //! this = Transpose(a)
 311                                 void                    Transpose(const Matrix3x3& a)
 312                                                                 {
 313                                                                         m[0][0] = a.m[0][0];    m[0][1] = a.m[1][0];    m[0][2] = a.m[2][0];
 314                                                                         m[1][0] = a.m[0][1];    m[1][1] = a.m[1][1];    m[1][2] = a.m[2][1];
 315                                                                         m[2][0] = a.m[0][2];    m[2][1] = a.m[1][2];    m[2][2] = a.m[2][2];
 316                                                                 }
 317
 318                 //! Compute the determinant of the matrix. We use the rule of Sarrus.
 319                                 float                   Determinant()                                   const
 320                                                                 {
 321                                                                         return (m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1])
 322                                                                                 -  (m[2][0]*m[1][1]*m[0][2] + m[2][1]*m[1][2]*m[0][0] + m[2][2]*m[1][0]*m[0][1]);
 323                                                                 }
 324 /*
 325                 //! Compute a cofactor. Used for matrix inversion.
 326                                 float                   CoFactor(ubyte row, ubyte column)       const
 327                                 {
 328                                         static sdword gIndex[3+2] = { 0, 1, 2, 0, 1 };
 329                                         return  (m[gIndex[row+1]][gIndex[column+1]]*m[gIndex[row+2]][gIndex[column+2]] - m[gIndex[row+2]][gIndex[column+1]]*m[gIndex[row+1]][gIndex[column+2]]);
 330                                 }
 331 */
 332                 //! Invert the matrix. Determinant must be different from zero, else matrix can't be inverted.
 333                                 Matrix3x3&              Invert()
 334                                                                 {
 335                                                                         float Det = Determinant();      // Must be !=0
 336                                                                         float OneOverDet = 1.0f / Det;
 337
 338                                                                         Matrix3x3 Temp;
 339                                                                         Temp.m[0][0] = +(m[1][1] * m[2][2] - m[2][1] * m[1][2]) * OneOverDet;
 340                                                                         Temp.m[1][0] = -(m[1][0] * m[2][2] - m[2][0] * m[1][2]) * OneOverDet;
 341                                                                         Temp.m[2][0] = +(m[1][0] * m[2][1] - m[2][0] * m[1][1]) * OneOverDet;
 342                                                                         Temp.m[0][1] = -(m[0][1] * m[2][2] - m[2][1] * m[0][2]) * OneOverDet;
 343                                                                         Temp.m[1][1] = +(m[0][0] * m[2][2] - m[2][0] * m[0][2]) * OneOverDet;
 344                                                                         Temp.m[2][1] = -(m[0][0] * m[2][1] - m[2][0] * m[0][1]) * OneOverDet;
 345                                                                         Temp.m[0][2] = +(m[0][1] * m[1][2] - m[1][1] * m[0][2]) * OneOverDet;
 346                                                                         Temp.m[1][2] = -(m[0][0] * m[1][2] - m[1][0] * m[0][2]) * OneOverDet;
 347                                                                         Temp.m[2][2] = +(m[0][0] * m[1][1] - m[1][0] * m[0][1]) * OneOverDet;
 348
 349                                                                         *this = Temp;
 350
 351                                                                         return  *this;
 352                                                                 }
 353
 354                                 Matrix3x3&              Normalize();
 355
 356                 //! this = exp(a)
 357                                 Matrix3x3&              Exp(const Matrix3x3& a);
 358
 359 void FromQuat(const Quat &q);
 360 void FromQuatL2(const Quat &q, float l2);
 361
 362                 // Arithmetic operators
 363                 //! Operator for Matrix3x3 Plus = Matrix3x3 + Matrix3x3;
 364                 inline_ Matrix3x3               operator+(const Matrix3x3& mat) const
 365                                                                 {
 366                                                                         return Matrix3x3(
 367                                                                         m[0][0] + mat.m[0][0],  m[0][1] + mat.m[0][1],  m[0][2] + mat.m[0][2],
 368                                                                         m[1][0] + mat.m[1][0],  m[1][1] + mat.m[1][1],  m[1][2] + mat.m[1][2],
 369                                                                         m[2][0] + mat.m[2][0],  m[2][1] + mat.m[2][1],  m[2][2] + mat.m[2][2]);
 370                                                                 }
 371
 372                 //! Operator for Matrix3x3 Minus = Matrix3x3 - Matrix3x3;
 373                 inline_ Matrix3x3               operator-(const Matrix3x3& mat) const
 374                                                                 {
 375                                                                         return Matrix3x3(
 376                                                                         m[0][0] - mat.m[0][0],  m[0][1] - mat.m[0][1],  m[0][2] - mat.m[0][2],
 377                                                                         m[1][0] - mat.m[1][0],  m[1][1] - mat.m[1][1],  m[1][2] - mat.m[1][2],
 378                                                                         m[2][0] - mat.m[2][0],  m[2][1] - mat.m[2][1],  m[2][2] - mat.m[2][2]);
 379                                                                 }
 380
 381                 //! Operator for Matrix3x3 Mul = Matrix3x3 * Matrix3x3;
 382                 inline_ Matrix3x3               operator*(const Matrix3x3& mat) const
 383                                                                 {
 384                                                                         return Matrix3x3(
 385                                                                         m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0],
 386                                                                         m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1],
 387                                                                         m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2],
 388
 389                                                                         m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0],
 390                                                                         m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1],
 391                                                                         m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2],
 392
 393                                                                         m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0],
 394                                                                         m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1],
 395                                                                         m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2]);
 396                                                                 }
 397
 398                 //! Operator for Point Mul = Matrix3x3 * Point;
 399                 inline_ Point                   operator*(const Point& v)               const           { return Point(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v); }
 400
 401                 //! Operator for Matrix3x3 Mul = Matrix3x3 * float;
 402                 inline_ Matrix3x3               operator*(float s)                              const
 403                                                                 {
 404                                                                         return Matrix3x3(
 405                                                                         m[0][0]*s,      m[0][1]*s,      m[0][2]*s,
 406                                                                         m[1][0]*s,      m[1][1]*s,      m[1][2]*s,
 407                                                                         m[2][0]*s,      m[2][1]*s,      m[2][2]*s);
 408                                                                 }
 409
 410                 //! Operator for Matrix3x3 Mul = float * Matrix3x3;
 411                 inline_ friend Matrix3x3 operator*(float s, const Matrix3x3& mat)
 412                                                                 {
 413                                                                         return Matrix3x3(
 414                                                                         s*mat.m[0][0],  s*mat.m[0][1],  s*mat.m[0][2],
 415                                                                         s*mat.m[1][0],  s*mat.m[1][1],  s*mat.m[1][2],
 416                                                                         s*mat.m[2][0],  s*mat.m[2][1],  s*mat.m[2][2]);
 417                                                                 }
 418
 419                 //! Operator for Matrix3x3 Div = Matrix3x3 / float;
 420                 inline_ Matrix3x3               operator/(float s)                              const
 421                                                                 {
 422                                                                         if (s)  s = 1.0f / s;
 423                                                                         return Matrix3x3(
 424                                                                         m[0][0]*s,      m[0][1]*s,      m[0][2]*s,
 425                                                                         m[1][0]*s,      m[1][1]*s,      m[1][2]*s,
 426                                                                         m[2][0]*s,      m[2][1]*s,      m[2][2]*s);
 427                                                                 }
 428
 429                 //! Operator for Matrix3x3 Div = float / Matrix3x3;
 430                 inline_ friend Matrix3x3 operator/(float s, const Matrix3x3& mat)
 431                                                                 {
 432                                                                         return Matrix3x3(
 433                                                                         s/mat.m[0][0],  s/mat.m[0][1],  s/mat.m[0][2],
 434                                                                         s/mat.m[1][0],  s/mat.m[1][1],  s/mat.m[1][2],
 435                                                                         s/mat.m[2][0],  s/mat.m[2][1],  s/mat.m[2][2]);
 436                                                                 }
 437
 438                 //! Operator for Matrix3x3 += Matrix3x3
 439                 inline_ Matrix3x3&              operator+=(const Matrix3x3& mat)
 440                                                                 {
 441                                                                         m[0][0] += mat.m[0][0];         m[0][1] += mat.m[0][1];         m[0][2] += mat.m[0][2];
 442                                                                         m[1][0] += mat.m[1][0];         m[1][1] += mat.m[1][1];         m[1][2] += mat.m[1][2];
 443                                                                         m[2][0] += mat.m[2][0];         m[2][1] += mat.m[2][1];         m[2][2] += mat.m[2][2];
 444                                                                         return  *this;
 445                                                                 }
 446
 447                 //! Operator for Matrix3x3 -= Matrix3x3
 448                 inline_ Matrix3x3&              operator-=(const Matrix3x3& mat)
 449                                                                 {
 450                                                                         m[0][0] -= mat.m[0][0];         m[0][1] -= mat.m[0][1];         m[0][2] -= mat.m[0][2];
 451                                                                         m[1][0] -= mat.m[1][0];         m[1][1] -= mat.m[1][1];         m[1][2] -= mat.m[1][2];
 452                                                                         m[2][0] -= mat.m[2][0];         m[2][1] -= mat.m[2][1];         m[2][2] -= mat.m[2][2];
 453                                                                         return  *this;
 454                                                                 }
 455
 456                 //! Operator for Matrix3x3 *= Matrix3x3
 457                 inline_ Matrix3x3&              operator*=(const Matrix3x3& mat)
 458                                                                 {
 459                                                                         Point TempRow;
 460
 461                                                                         GetRow(0, TempRow);
 462                                                                         m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];
 463                                                                         m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];
 464                                                                         m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];
 465
 466                                                                         GetRow(1, TempRow);
 467                                                                         m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];
 468                                                                         m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];
 469                                                                         m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];
 470
 471                                                                         GetRow(2, TempRow);
 472                                                                         m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0];
 473                                                                         m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1];
 474                                                                         m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2];
 475                                                                         return  *this;
 476                                                                 }
 477
 478                 //! Operator for Matrix3x3 *= float
 479                 inline_ Matrix3x3&              operator*=(float s)
 480                                                                 {
 481                                                                         m[0][0] *= s;   m[0][1] *= s;   m[0][2] *= s;
 482                                                                         m[1][0] *= s;   m[1][1] *= s;   m[1][2] *= s;
 483                                                                         m[2][0] *= s;   m[2][1] *= s;   m[2][2] *= s;
 484                                                                         return  *this;
 485                                                                 }
 486
 487                 //! Operator for Matrix3x3 /= float
 488                 inline_ Matrix3x3&              operator/=(float s)
 489                                                                 {
 490                                                                         if (s)  s = 1.0f / s;
 491                                                                         m[0][0] *= s;   m[0][1] *= s;   m[0][2] *= s;
 492                                                                         m[1][0] *= s;   m[1][1] *= s;   m[1][2] *= s;
 493                                                                         m[2][0] *= s;   m[2][1] *= s;   m[2][2] *= s;
 494                                                                         return  *this;
 495                                                                 }
 496
 497                 // Cast operators
 498                 //! Cast a Matrix3x3 to a Matrix4x4.
 499                                                                 operator Matrix4x4()    const;
 500                 //! Cast a Matrix3x3 to a Quat.
 501                                                                 operator Quat()                 const;
 502
 503                 inline_ const Point&    operator[](int row)             const   { return *(const Point*)&m[row][0];     }
 504                 inline_ Point&                  operator[](int row)                             { return *(Point*)&m[row][0];           }
 505
 506                 public:
 507
 508                                 float                   m[3][3];
 509         };
 510
 511 #endif // __ICEMATRIX3X3_H__
 512