The Ecere SDK - sdk/blob - ecere/src/gfx/3D/Matrix.ec

   1 namespace gfx3D;
   2
   3 import "Display"
   4
   5 public union Matrix
   6 {
   7    double array[16];
   8    double m[4][4];
   9
  10    const char * OnGetString(char * string, void * fieldData, bool * needClass)
  11    {
  12       int y, x;
  13       string[0] = 0;
  14       strcat(string, "{ ");
  15       for(y = 0; y < 4; y++)
  16       {
  17          strcat(string, "{ ");
  18          for(x = 0; x < 4; x++)
  19          {
  20             char member[256];
  21             const char * s = m[y][x].OnGetString(member, null, null);
  22             strcat(string, s);
  23             if(x < 3) strcat(string, ", ");
  24          }
  25          strcat(string, " }");
  26          if(y < 3) strcat(string, ", ");
  27       }
  28       strcat(string, " }");
  29       return string;
  30    }
  31
  32    void Identity()
  33    {
  34       FillBytesBy4(this, 0, sizeof(Matrix) >> 2);
  35       m[0][0]=m[1][1]=m[2][2]=m[3][3]=1;
  36    }
  37
  38    void Transpose(Matrix source)
  39    {
  40       int i,j;
  41       for(i=0; i<4; i++)
  42          for(j=0; j<4; j++)
  43             m[j][i] = source.m[i][j];
  44    }
  45
  46    void Multiply(Matrix a, Matrix b)
  47    {
  48 #ifdef __ANDROID__
  49       // We need a full matrix multiplication for the Projection matrix
  50       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] + a.m[0][3]*b.m[3][0];
  51       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] + a.m[0][3]*b.m[3][1];
  52       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] + a.m[0][3]*b.m[3][2];
  53       m[0][3]=a.m[0][0]*b.m[0][3] + a.m[0][1]*b.m[1][3] + a.m[0][2]*b.m[2][3] + a.m[0][3]*b.m[3][3];
  54
  55       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] + a.m[1][3]*b.m[3][0];
  56       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] + a.m[1][3]*b.m[3][1];
  57       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] + a.m[1][3]*b.m[3][2];
  58       m[1][3]=a.m[1][0]*b.m[0][3] + a.m[1][1]*b.m[1][3] + a.m[1][2]*b.m[2][3] + a.m[1][3]*b.m[3][3];
  59
  60       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] + a.m[2][3]*b.m[3][0];
  61       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] + a.m[2][3]*b.m[3][1];
  62       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] + a.m[2][3]*b.m[3][2];
  63       m[2][3]=a.m[2][0]*b.m[0][3] + a.m[2][1]*b.m[1][3] + a.m[2][2]*b.m[2][3] + a.m[2][3]*b.m[3][3];
  64
  65       m[3][0]=a.m[3][0]*b.m[0][0] + a.m[3][1]*b.m[1][0] + a.m[3][2]*b.m[2][0] + a.m[3][3]*b.m[3][0];
  66       m[3][1]=a.m[3][0]*b.m[0][1] + a.m[3][1]*b.m[1][1] + a.m[3][2]*b.m[2][1] + a.m[3][3]*b.m[3][1];
  67       m[3][2]=a.m[3][0]*b.m[0][2] + a.m[3][1]*b.m[1][2] + a.m[3][2]*b.m[2][2] + a.m[3][3]*b.m[3][2];
  68       m[3][3]=a.m[3][0]*b.m[0][3] + a.m[3][1]*b.m[1][3] + a.m[3][2]*b.m[2][3] + a.m[3][3]*b.m[3][3];
  69 #else
  70       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];
  71       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];
  72       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];
  73       m[0][3]=0;
  74
  75       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];
  76       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];
  77       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];
  78       m[1][3]=0;
  79
  80       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];
  81       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];
  82       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];
  83       m[2][3]=0;
  84
  85       m[3][0]=a.m[3][0]*b.m[0][0] + a.m[3][1]*b.m[1][0] + a.m[3][2]*b.m[2][0] + b.m[3][0];
  86       m[3][1]=a.m[3][0]*b.m[0][1] + a.m[3][1]*b.m[1][1] + a.m[3][2]*b.m[2][1] + b.m[3][1];
  87       m[3][2]=a.m[3][0]*b.m[0][2] + a.m[3][1]*b.m[1][2] + a.m[3][2]*b.m[2][2] + b.m[3][2];
  88       m[3][3]=1;
  89 #endif
  90    }
  91
  92    void RotationQuaternion(Quaternion quat)
  93    {
  94       double xx = quat.x*quat.x, yy = quat.y*quat.y, zz = quat.z*quat.z;
  95       double xy = quat.x*quat.y, xz = quat.x*quat.z, yz = quat.y*quat.z;
  96       double wx = quat.w*quat.x, wy = quat.w*quat.y, wz = quat.w*quat.z;
  97
  98       m[0][0] = (double) (1 - 2 * ( yy + zz ));
  99       m[0][1] = (double) (    2 * ( xy - wz ));
 100       m[0][2] = (double) (    2 * ( xz + wy ));
 101
 102       m[1][0] = (double) (    2 * ( xy + wz ));
 103       m[1][1] = (double) (1 - 2 * ( xx + zz ));
 104       m[1][2] = (double) (    2 * ( yz - wx ));
 105
 106       m[2][0] = (double) (    2 * ( xz - wy ));
 107       m[2][1] = (double) (    2 * ( yz + wx ));
 108       m[2][2] = (double) (1 - 2 * ( xx + yy ));
 109
 110       m[0][3] = m[1][3] = m[2][3] = 0.0f;
 111       m[3][0] = m[3][1] = m[3][2] = 0.0f;
 112       m[3][3] = 1.0f;
 113    }
 114
 115    void Translate(double tx, double ty, double tz)
 116    {
 117       Matrix tmat;
 118       Matrix mat1;
 119
 120       FillBytesBy4(tmat, 0, sizeof(Matrix) >> 2);
 121       tmat.m[0][0]=tmat.m[1][1]=tmat.m[2][2]=tmat.m[3][3]=1;
 122       tmat.m[3][0]=tx; tmat.m[3][1]=ty; tmat.m[3][2]=tz;
 123       mat1.Multiply(this, tmat);
 124       this = mat1;
 125    }
 126
 127    void Scale(double sx, double sy, double sz)
 128    {
 129       Matrix smat;
 130       Matrix mat1;
 131       FillBytesBy4(smat, 0, sizeof(Matrix) >> 2);
 132       smat.m[0][0]=sx; smat.m[1][1]=sy; smat.m[2][2]=sz; smat.m[3][3]=1;
 133       mat1.Multiply(this, smat);
 134       this = mat1;
 135    }
 136
 137    void Rotate(Quaternion quat)
 138    {
 139       Matrix rmat;
 140       Matrix mat1;
 141       rmat.RotationQuaternion(quat);
 142       mat1.Multiply(this, rmat);
 143       this = mat1;
 144    }
 145
 146    double Determinant(void)
 147    {
 148       double result = 0, i = 1;
 149       int n;
 150       for(n = 0; n < 4; n++, i *= -1)
 151       {
 152          double det;
 153          double msub3[3][3];
 154          int di, dj;
 155
 156          for(di = 0; di < 3; di++)
 157          {
 158             for(dj = 0; dj < 3; dj++)
 159             {
 160                int si = di + ( ( di >= 0 ) ? 1 : 0 );
 161                int sj = dj + ( ( dj >= n ) ? 1 : 0 );
 162                msub3[di][dj] = m[si][sj];
 163             }
 164          }
 165          det = msub3[0][0] * ( msub3[1][1]*msub3[2][2] - msub3[2][1]*msub3[1][2] )
 166              - msub3[0][1] * ( msub3[1][0]*msub3[2][2] - msub3[2][0]*msub3[1][2] )
 167              + msub3[0][2] * ( msub3[1][0]*msub3[2][1] - msub3[2][0]*msub3[1][1] );
 168
 169          result += m[0][n] * det * i;
 170       }
 171       return result;
 172    }
 173
 174    void Inverse(Matrix source)
 175    {
 176       double det = source.Determinant();
 177       // if(Abs(det) < 0.0005)
 178       if(Abs(det) < 0.00000001)
 179          Identity();
 180       else
 181       {
 182          int i, j, sign;
 183          for ( i = 0; i < 4; i++ )
 184             for ( j = 0; j < 4; j++ )
 185             {
 186                int di, dj;
 187                double msub3[3][3];
 188                double m3det;
 189
 190                sign = 1 - ( (i+j) % 2 ) * 2;
 191
 192                for(di = 0; di < 3; di++)
 193                {
 194                   for(dj = 0; dj < 3; dj++)
 195                   {
 196                      int si = di + ( ( di >= i ) ? 1 : 0 );
 197                      int sj = dj + ( ( dj >= j ) ? 1 : 0 );
 198                      msub3[di][dj] = source.m[si][sj];
 199                   }
 200                }
 201                m3det = msub3[0][0] * ( msub3[1][1]*msub3[2][2] - msub3[2][1]*msub3[1][2] )
 202                      - msub3[0][1] * ( msub3[1][0]*msub3[2][2] - msub3[2][0]*msub3[1][2] )
 203                      + msub3[0][2] * ( msub3[1][0]*msub3[2][1] - msub3[2][0]*msub3[1][1] );
 204
 205                m[j][i] = m3det * sign / det;
 206             }
 207       }
 208    }
 209
 210    void ToEuler(Euler euler)
 211    {
 212       if(fabs(m[2][1]) <= 1.0 - 0.000005)
 213       {
 214          euler.yaw   = atan2(-m[2][0], m[2][2]);
 215          euler.pitch = asin ( m[2][1]);
 216          euler.roll  = atan2(-m[0][1], m[1][1]);
 217       }
 218       else
 219       {
 220          euler.yaw = acos(m[0][0]);
 221          euler.pitch = Pi/2;
 222          euler.roll = 0;
 223       }
 224    }
 225
 226    property Quaternion
 227    {
 228       set { RotationQuaternion(value); }
 229       get { value.RotationMatrix(this); }
 230    }
 231 };