Advertisement
Guest User

Untitled

a guest
Apr 29th, 2010
246
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 1.92 KB | None | 0 0
  1.     //D contains 12 determinants of 2x2 submatrices of A chosen by convenience
  2.     //C contains the (+/-)determinants of 3x3 submatrices created by elimination of given row and column from A
  3.     static void Cofactor(float* C, const float* A)
  4.     {
  5.         float D[12] = {
  6.             /* 0*/  A[ 0]*A[ 5] - A[ 4]*A[ 1],
  7.             /* 1*/  A[ 0]*A[ 6] - A[ 4]*A[ 2],
  8.             /* 2*/  A[ 0]*A[ 7] - A[ 4]*A[ 3],
  9.             /* 3*/  A[ 1]*A[ 6] - A[ 5]*A[ 2],
  10.             /* 4*/  A[ 1]*A[ 7] - A[ 5]*A[ 3],
  11.             /* 5*/  A[ 2]*A[ 7] - A[ 6]*A[ 3],
  12.             /* 6*/  A[ 8]*A[13] - A[12]*A[ 9],
  13.             /* 7*/  A[ 8]*A[14] - A[12]*A[10],
  14.             /* 8*/  A[ 8]*A[15] - A[12]*A[11],
  15.             /* 9*/  A[ 9]*A[14] - A[13]*A[10],
  16.             /*10*/  A[ 9]*A[15] - A[13]*A[11],
  17.             /*11*/  A[10]*A[15] - A[14]*A[11]
  18.         };
  19.  
  20.         C[ 0] = + A[ 5]*D[11] - A[ 6]*D[10] + A[ 7]*D[ 9];
  21.         C[ 1] = - A[ 4]*D[11] + A[ 6]*D[ 8] - A[ 7]*D[ 7];
  22.         C[ 2] = + A[ 4]*D[10] - A[ 5]*D[ 8] + A[ 7]*D[ 6];
  23.         C[ 3] = - A[ 4]*D[ 9] + A[ 5]*D[ 7] - A[ 6]*D[ 6];
  24.         C[ 4] = - A[ 1]*D[11] + A[ 2]*D[10] - A[ 3]*D[ 9];
  25.         C[ 5] = + A[ 0]*D[11] - A[ 2]*D[ 8] + A[ 3]*D[ 7];
  26.         C[ 6] = - A[ 0]*D[10] + A[ 1]*D[ 8] - A[ 3]*D[ 6];
  27.         C[ 7] = + A[ 0]*D[ 9] - A[ 1]*D[ 7] + A[ 2]*D[ 6];
  28.         C[ 8] = + A[13]*D[ 5] - A[14]*D[ 4] + A[15]*D[ 3];
  29.         C[ 9] = - A[12]*D[ 5] + A[14]*D[ 2] - A[15]*D[ 1];
  30.         C[10] = + A[12]*D[ 4] - A[13]*D[ 2] + A[15]*D[ 0];
  31.         C[11] = - A[12]*D[ 3] + A[13]*D[ 1] - A[14]*D[ 0];
  32.         C[12] = - A[ 9]*D[ 5] + A[10]*D[ 4] - A[11]*D[ 3];
  33.         C[13] = + A[ 8]*D[ 5] - A[10]*D[ 2] + A[11]*D[ 1];
  34.         C[14] = - A[ 8]*D[ 4] + A[ 9]*D[ 2] - A[11]*D[ 0];
  35.         C[15] = + A[ 8]*D[ 3] - A[ 9]*D[ 1] + A[10]*D[ 0];
  36.     }
  37.  
  38.     // Invert(A, A) is alright
  39.     static bool Invert(float* R, const float* A)
  40.     {
  41.         float C[16];
  42.         float D;
  43.  
  44.         Cofactor(C, A);
  45.  
  46.         D = A[0]*C[0] + A[1]*C[1] + A[2]*C[2] + A[3]*C[3];
  47.  
  48.         if(D != 0.0f)
  49.         {
  50.             Transpose(R, C);                // Copies the adjacent matrix of A to R
  51.             MultScalar(R, 1.0f / D);        // Divides R by the det(A)
  52.  
  53.             return true;
  54.         }
  55.        
  56.         return false;
  57.     }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement