OPCODE/Ice/IceMatrix4x4.cpp

   1 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   2 /**
   3  *      Contains code for 4x4 matrices.
   4  *      \file           IceMatrix4x4.cpp
   5  *      \author         Pierre Terdiman
   6  *      \date           April, 4, 2000
   7  */
   8 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   9
  10 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  11 /**
  12  *      4x4 matrix.
  13  *      DirectX-compliant, ie row-column order, ie m[Row][Col].
  14  *      Same as:
  15  *      m11  m12  m13  m14      first row.
  16  *      m21  m22  m23  m24      second row.
  17  *      m31  m32  m33  m34      third row.
  18  *      m41  m42  m43  m44      fourth row.
  19  *      Translation is (m41, m42, m43), (m14, m24, m34, m44) = (0, 0, 0, 1).
  20  *      Stored in memory as m11 m12 m13 m14 m21...
  21  *
  22  *      Multiplication rules:
  23  *
  24  *      [x'y'z'1] = [xyz1][M]
  25  *
  26  *      x' = x*m11 + y*m21 + z*m31 + m41
  27  *      y' = x*m12 + y*m22 + z*m32 + m42
  28  *      z' = x*m13 + y*m23 + z*m33 + m43
  29  *      1' =     0 +     0 +     0 + m44
  30  *
  31  *      \class          Matrix4x4
  32  *      \author         Pierre Terdiman
  33  *      \version        1.0
  34  */
  35 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  36
  37 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  38 // Precompiled Header
  39 #include "Stdafx.h"
  40
  41 using namespace IceMaths;
  42
  43 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  44 /**
  45  *      Inverts a PR matrix. (which only contains a rotation and a translation)
  46  *      This is faster and less subject to FPU errors than the generic inversion code.
  47  *
  48  *      \relates        Matrix4x4
  49  *      \fn                     InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
  50  *      \param          dest    [out] destination matrix
  51  *      \param          src             [in] source matrix
  52  */
  53 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  54 ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
  55 {
  56         dest.m[0][0] = src.m[0][0];
  57         dest.m[1][0] = src.m[0][1];
  58         dest.m[2][0] = src.m[0][2];
  59         dest.m[3][0] = -(src.m[3][0]*src.m[0][0] + src.m[3][1]*src.m[0][1] + src.m[3][2]*src.m[0][2]);
  60
  61         dest.m[0][1] = src.m[1][0];
  62         dest.m[1][1] = src.m[1][1];
  63         dest.m[2][1] = src.m[1][2];
  64         dest.m[3][1] = -(src.m[3][0]*src.m[1][0] + src.m[3][1]*src.m[1][1] + src.m[3][2]*src.m[1][2]);
  65
  66         dest.m[0][2] = src.m[2][0];
  67         dest.m[1][2] = src.m[2][1];
  68         dest.m[2][2] = src.m[2][2];
  69         dest.m[3][2] = -(src.m[3][0]*src.m[2][0] + src.m[3][1]*src.m[2][1] + src.m[3][2]*src.m[2][2]);
  70
  71         dest.m[0][3] = 0.0f;
  72         dest.m[1][3] = 0.0f;
  73         dest.m[2][3] = 0.0f;
  74         dest.m[3][3] = 1.0f;
  75 }
  76
  77 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  78 // Compute the cofactor of the Matrix at a specified location
  79 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  80 float Matrix4x4::CoFactor(udword row, udword col) const
  81 {
  82         return   (( m[(row+1)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+3)&3][(col+3)&3] +
  83                                 m[(row+1)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+3)&3][(col+1)&3] +
  84                                 m[(row+1)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+3)&3][(col+2)&3])
  85                         -  (m[(row+3)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+1)&3][(col+3)&3] +
  86                                 m[(row+3)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+1)&3][(col+1)&3] +
  87                                 m[(row+3)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+1)&3][(col+2)&3])) * ((row + col) & 1 ? -1.0f : +1.0f);
  88 }
  89
  90 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  91 // Compute the determinant of the Matrix
  92 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  93 float Matrix4x4::Determinant() const
  94 {
  95         return  m[0][0] * CoFactor(0, 0) +
  96                         m[0][1] * CoFactor(0, 1) +
  97                         m[0][2] * CoFactor(0, 2) +
  98                         m[0][3] * CoFactor(0, 3);
  99 }
 100
 101 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 102 // Compute the inverse of the matrix
 103 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 104 Matrix4x4& Matrix4x4::Invert()
 105 {
 106         float Det = Determinant();
 107         Matrix4x4 Temp;
 108
 109         if(fabsf(Det) < MATRIX4X4_EPSILON)
 110                 return  *this;          // The matrix is not invertible! Singular case!
 111
 112         float IDet = 1.0f / Det;
 113
 114         Temp.m[0][0] = CoFactor(0,0) * IDet;
 115         Temp.m[1][0] = CoFactor(0,1) * IDet;
 116         Temp.m[2][0] = CoFactor(0,2) * IDet;
 117         Temp.m[3][0] = CoFactor(0,3) * IDet;
 118         Temp.m[0][1] = CoFactor(1,0) * IDet;
 119         Temp.m[1][1] = CoFactor(1,1) * IDet;
 120         Temp.m[2][1] = CoFactor(1,2) * IDet;
 121         Temp.m[3][1] = CoFactor(1,3) * IDet;
 122         Temp.m[0][2] = CoFactor(2,0) * IDet;
 123         Temp.m[1][2] = CoFactor(2,1) * IDet;
 124         Temp.m[2][2] = CoFactor(2,2) * IDet;
 125         Temp.m[3][2] = CoFactor(2,3) * IDet;
 126         Temp.m[0][3] = CoFactor(3,0) * IDet;
 127         Temp.m[1][3] = CoFactor(3,1) * IDet;
 128         Temp.m[2][3] = CoFactor(3,2) * IDet;
 129         Temp.m[3][3] = CoFactor(3,3) * IDet;
 130
 131         *this = Temp;
 132
 133         return  *this;
 134 }
 135