Mindboards

Give a child a robot, he'll play for an hour. Teach him to build, and he'll play forever.
https://mindboards.org:443/

NXC: Matrix algebra library

https://mindboards.org:443/viewtopic.php?t=947

Page 1 of 6

NXC: Matrix algebra library

Posted: 11 Jul 2011, 19:24
by HaWe
hi,
here's my matrix algebra library - share and enjoy!
latest untweaked version:

Code: Select all

// Matrix algebra / arithmetics
// vers. 1.10a


// Matrix A * scalar: <=>    A*=scalar;

// Matrix A + Matrix B: <=>  A+= B;


#define ArrayInit2D(array, tmp, init_val, dimx, dimy) { \
  ArrayInit(tmp, init_val, dimy);  \
  ArrayInit(array, tmp, dimx);     \
  ArrayInit(tmp,0,0);              \
}

                                                      // [n , m] x [m , k]
void MatrixMatrixMult(float A[][], float B[][], float &C[][], int N, int M, int K){
  int i, j, s;
  float tmp[];
  ArrayInit2D(C,tmp, 0, N, K);
  for (i=0; i<N; ++i) {
    for (j=0; j<K; ++j) {
       C[i][j]=0;
       for (s=0; s<M; ++s) {
         C[i][j]=C[i][j] + A[i][s]*B[s][j];
      }
    }
  }
}


                                                // Matrix[n,m] x Vector[m]
void MatrixVectorMult(float A[][], float B[], float &C[], int N, int M){   
  int i, s;
  ArrayInit(C,0, M);
  for (i=0; i<M; ++i) {
     C[i]=0;
     for (s=0; s<N; ++s) {
       C[i]=C[i] + A[i][s]*B[s];
     }
  }
}

                                                 // Vector[n]^T x Matrix [n,m]
void VectorMatrixMult(float A[], float B[][], float &C[], int N, int M){    // K=1
  int j, s;
  ArrayInit(C,0, M);
  for (j=0; j<M; ++j) {
     C[j]=0;
     for (s=0; s<N; ++s) {
       C[j]=C[j] + A[s]*B[s][j];
     }
  }
}


void MatrixDiag(float A[][], float &D[][], int RS) {   // Diagonal matrix
   float tmp[], s;
   ArrayInit2D(D, tmp, 0, RS,RS);
   for (s=0; s<RS; s++) {
     D[s][s]=A[s][s];
   }
}


                                                // Transposed / Transponierte
void MatrixTransp(float A[][], float &C[][], int R, int S) {
   int r, s;
   float tmp[];
   ArrayInit2D(C,tmp, 0, S,R);

   for (s=0; s<S; s++) {
     for (r=0; r<R; r++) {
       C[s][r]=A[r][s];
     }
   }
}


void VectorTransp(float A[], float &C[][], int R) {
   int r;
   float tmp[];
   ArrayInit2D(C,tmp, 0, 1,R);
   for (r=0; r<R; r++) {
     C[0][r]=A[r];
   }
}


void MatrixRotate2DMath(float A[][], float &C[][], float angleDeg) {
   float tmp[], x, B[][];

   ArrayInit2D(C, tmp, 0, 2, 2);

   x=cosd(angleDeg);
   ArrayInit2D(B, tmp, x, 2, 2);
   x=sind(angleDeg);
   B[0][1]=-x;
   B[1][0]= x;

   MatrixMatrixMult(B, A, C, 2, 2, 2);
}


void VectorRotate2DMath(float A[], float &C[], float angleDeg) {
   float tmp[], x, B[][];

   ArrayInit(C, 0, 2);

   x=cosd(angleDeg);
   ArrayInit2D(B, tmp, x, 2, 2);
   x=sind(angleDeg);
   B[0][1]=-x;
   B[1][0]= x;

   MatrixVectorMult(B, A, C, 2, 2);
}


void MatrixRotate2DGeo(float A[][], float &C[][], float angleDeg) {
   float tmp[], x, B[][];
   angleDeg*=-1;
   ArrayInit2D(C, tmp, 0, 2, 2);

   x=cosd(angleDeg);
   ArrayInit2D(B, tmp, x, 2, 2);
   x=sind(angleDeg);
   B[0][1]=-x;
   B[1][0]= x;

   MatrixMatrixMult(B, A, C, 2, 2, 2);
}


void VectorRotate2DGeo(float A[], float &C[], float angleDeg) {
   float tmp[], x, B[][];
   angleDeg*=-1;
   ArrayInit(C, 0, 2);

   x=cosd(angleDeg);
   ArrayInit2D(B, tmp, x, 2, 2);
   x=sind(angleDeg);
   B[0][1]=-x;
   B[1][0]= x;

   MatrixVectorMult(B, A, C, 2, 2);
}



float MatrixDet1x1(float A[]) {                 // Determinante 1x1
   return ( A[0]);
}


float MatrixDet2x2(float A[][]) {               // Determinante 2x2
   return ( A[0][0]*A[1][1]- A[0][1]*A[1][0] );
}


float MatrixDet3x3(float A[][]) {               // Determinante 3x3
   return (A[0][0]*A[1][1]*A[2][2]
          +A[0][1]*A[1][2]*A[2][0]
          +A[0][2]*A[1][0]*A[2][1]
          -A[0][2]*A[1][1]*A[2][0]
          -A[0][1]*A[1][0]*A[2][2]
          -A[0][0]*A[1][2]*A[2][1]);
}


void MatrixAdj1x1 (float A[], float &C[]) {     // Adjugate=Adjunkte 1x1
  ArrayInit(C, 1, 1);
}


void MatrixAdj2x2 (float A[][], float &C[][]) {  // Adjugate=Adjunkte 2x2
  float tmp[];
  ArrayInit2D(C, tmp, 0, 2,2);
  C[0][0]=  A[1][1];   C[1][0]= -A[1][0];
  C[0][1]= -A[0][1];   C[1][1]=  A[0][0];
}


void MatrixAdj3x3 (float A[][], float &C[][]) {  // Adjugate=Adjunkte 3x3
   float tmp[];
   ArrayInit2D(C, tmp, 0, 3,3);
   C[0][0]=A[1][1]*A[2][2]-A[1][2]*A[2][1];  C[0][1]=A[0][2]*A[2][1]-A[0][1]*A[2][2];  C[0][2]=A[0][1]*A[1][2]-A[0][2]*A[1][1];

   C[1][0]=A[1][2]*A[2][0]-A[1][0]*A[2][2];  C[1][1]=A[0][0]*A[2][2]-A[0][2]*A[2][0];  C[1][2]=A[0][2]*A[1][0]-A[0][0]*A[1][2];

   C[2][0]=A[1][0]*A[2][1]-A[1][1]*A[2][0];  C[2][1]=A[0][1]*A[2][0]-A[0][0]*A[2][1];  C[2][2]=A[0][0]*A[1][1]-A[0][1]*A[1][0];
}


bool MatrixInverse1x1 (float A[], float &C[]) {
   ArrayInit(C, 0, 1);
   if (C[0]==0) return 0;
   else {
     C[0]=1/A[0];
     return 1;
   }
}

bool MatrixInverse2x2 (float A[][], float &C[][]) {
   float det;
   float tmp[];
   ArrayInit2D(C, tmp, 0, 2,2);

   det=MatrixDet2x2(A);
   if (det==0) return 0;
   else {
     MatrixAdj2x2(A, C);
     C/=det;
     return 1;
   }
}


bool MatrixInverse3x3 (float A[][], float &C[][]) {
   float det;
   float tmp[];
   ArrayInit2D(C, tmp, 0, 3,3);

   det=MatrixDet3x3(A);
   if (det==0) return 0;
   else {
     MatrixAdj3x3(A, C);
     C/=det;
     return 1;
   }
}

void MatrixLinear(float A[][], float &C[], int R, int S) {
  int i=0, s, r;
  int dim=R*S;
  ArrayInit(C, 0, dim);
  for (s=0; s<S; ++s) {
    for (r=0; r<R; ++r) {
      C[i++]=A[r][s];
    }
  }
}


void SubMatrix(float A[][], float &C[][], int RS, int r, int s) {
  int i=0, j=0, x=0, y=0, dim_1;
  float tmp[];

  dim_1=RS-1;
  ArrayInit2D(C, tmp, 0, dim_1, dim_1);

  for (y=0; y<RS; ++y) {
    for (x=0; x<RS; ++x) {
      if ((x!=r)&&(y!=s))   {
         C[i][j]=A[x][y];
         ++i;
         if (i>=dim_1){
           i=0; ++j;
         }
      }
    }
  }
}

float MatrixMinor(float A[][], int RS, int r, int s) {
   float C[][];

   SubMatrix(A, C, RS, r, s);

   if(RS==3) {
     return MatrixDet2x2(C);
   }
   else
   if(RS==4) {
     return MatrixDet3x3(C);
   }
}


float MatrixCofactor(float A[][], int RS, int r, int s) {
   float x;

   return pow(-1, r+s)* MatrixMinor(A, RS, r, s);;
}


float MatrixDet4x4(float A[][])  {
   float det=0, x=0;
   int i, j=0, n0, max0=0, imax0, jmax0=0;

   for (j=0; j<4; ++j) {    // find column with most 0's: jmax0
     n0=0;
     for (i=0; i<4; ++i) {
       if (A[i][j]==0) ++n0;
     }
     if (max0<n0) {max0=n0; jmax0=j;}
   }

   for (i=0; i<4; ++i) {    // find row with most 0's: imax0
     n0=0;
     for (j=0; j<4; ++j) {
       if (A[i][j]==0) ++n0;
     }
     if (max0<n0) {max0=n0; imax0=i;}
   }


   if (jmax0>imax0) {
     for (i=0; i<4; ++i) {         // develop from jmax0
       if ( A[i][jmax0]!=0) {      // 0*?=0
          x=A[i][jmax0]*MatrixCofactor(A, 4, i, jmax0);
          det+=x;
       }
     }
     //TextOut(72,56,"j"); NumOut(80,56,jmax0); //debug
     return det;
   }
   else  {
     for (j=0; j<4; ++j) {         // develop from imax0
       if ( A[imax0][j]!=0) {      // 0*?=0
          x=A[imax0][j]*MatrixCofactor(A, 4, imax0, j);
          det+=x;
       }
     }
     //TextOut(72,56,"i"); NumOut(80,56,imax0); //debug
     return det;
   }
}

task main() {
  float A[2][2], B[2][2], C[2][2], x;
  float O[4][4], T[][], L[][];


  O[0][0]=0;   O[0][1]=0;   O[0][2]=8;   O[0][3]=22;
  O[1][0]=1;   O[1][1]=5;   O[1][2]=9;   O[1][3]=13;
  O[2][0]=2;   O[2][1]=6;   O[2][2]=10;  O[2][3]=14;
  O[3][0]=3;   O[3][1]=7;   O[3][2]=22;  O[3][3]=15;

  NumOut(0,56,O[0][0]);  NumOut(18,56,O[0][1]);  NumOut(36,56,O[0][2]);  NumOut(54,56,O[0][3]);
  NumOut(0,48,O[1][0]);  NumOut(18,48,O[1][1]);  NumOut(36,48,O[1][2]);  NumOut(54,48,O[1][3]);
  NumOut(0,40,O[2][0]);  NumOut(18,40,O[2][1]);  NumOut(36,40,O[2][2]);  NumOut(54,40,O[2][3]);
  NumOut(0,32,O[3][0]);  NumOut(18,32,O[3][1]);  NumOut(36,32,O[3][2]);  NumOut(54,32,O[3][3]);
                                                 TextOut(72,48,"det");
  x=MatrixDet4x4(O);                             NumOut(72,40,x);        /*  968   */

  SubMatrix(O,T,4, 2,1);  // expanded to A[i][j]
  TextOut(0,24,"SubMatrix[2;1]");
  NumOut(0,16,T[0][0]);  NumOut(24,16,T[0][1]);  NumOut(48,16,T[0][2]);
  NumOut(0,08,T[1][0]);  NumOut(24,08,T[1][1]);  NumOut(48, 8,T[1][2]);
  NumOut(0, 0,T[2][0]);  NumOut(24, 0,T[2][1]);  NumOut(48, 0,T[2][2]);

  while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();

  NumOut(0,56,T[0][0]);  NumOut(24,56,T[0][1]);  NumOut(48,56,T[0][2]);   TextOut(72,56,"det");
  NumOut(0,48,T[1][0]);  NumOut(24,48,T[1][1]);  NumOut(48,48,T[1][2]);   NumOut (72,48, MatrixDet3x3(T));
  NumOut(0,40,T[2][0]);  NumOut(24,40,T[2][1]);  NumOut(48,40,T[2][2]);   /*  82   */

  SubMatrix(T,C,3, 0,1);
  TextOut(0,32,"SubMatrix[0;1]");
  NumOut(0,24,C[0][0]);  NumOut(24,24,C[0][1]);    /* 1   13 */           TextOut(48,24,"det");
  NumOut(0,16,C[1][0]);  NumOut(24,16,C[1][1]);    /* 3   15 */           x=MatrixDet2x2(C);
                                                                          NumOut (72,24, x);
                                                                          /* 15-(3*13)= -24     */

  TextOut(0,8,"MMinor[0;1]");                                             /* TEST: */
  x=MatrixMinor(T, 3, 0,1);                       NumOut (72, 8, x);      /* Minor T[0,1]= det(C)= -24  */
  TextOut(0,0,"MCofac[0;1]");
  x=MatrixCofactor(T, 3, 0,1);                    NumOut (72, 0, x);      /* Cof T[0,1]= det(C)*(-1)^(0+1) = +24 */



  while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();


  A[0][0]=1;   A[0][1]=3;
  A[1][0]=2;   A[1][1]=4;
  NumOut(0,56,A[0][0]);  NumOut(30,56,A[0][1]);
  NumOut(0,48,A[1][0]);  NumOut(30,48,A[1][1]);

  TextOut(0,40,"B=A*10");
  x=10;
  B=A; B*=x;
  NumOut(0,32,B[0][0]);  NumOut(30,32,B[0][1]);
  NumOut(0,24,B[1][0]);  NumOut(30,24,B[1][1]);

  TextOut(0,16,"C=A+B");
  C=A; C+=B;
  NumOut(0,08,C[0][0]);  NumOut(30,08,C[0][1]);
  NumOut(0,00,C[1][0]);  NumOut(30,00,C[1][1]);


  while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();

  NumOut(0,56,A[0][0]);  NumOut(30,56,A[0][1]);
  NumOut(0,48,A[1][0]);  NumOut(30,48,A[1][1]);

  TextOut(0,40,"Adjunkte");
  MatrixAdj2x2(A,C);
  NumOut(0,32,C[0][0]);  NumOut(30,32,C[0][1]);
  NumOut(0,24,C[1][0]);  NumOut(30,24,C[1][1]);

  TextOut(0,16,"Inverse");
  MatrixInverse2x2(A,C);
  NumOut(0,08,C[0][0]);  NumOut(30,08,C[0][1]);
  NumOut(0,00,C[1][0]);  NumOut(30,00,C[1][1]);

  while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();

  NumOut(0,56,A[0][0]);  NumOut(30,56,A[0][1]);
  NumOut(0,48,A[1][0]);  NumOut(30,48,A[1][1]);

  TextOut(0,40,"Inverse");
  MatrixInverse2x2(A,B);
  NumOut(0,32,B[0][0]);  NumOut(30,32,B[0][1]);
  NumOut(0,24,B[1][0]);  NumOut(30,24,B[1][1]);

  TextOut(0,16,"Matrix * Inverse");
  MatrixMatrixMult(A,B,C,2,2,2);
  NumOut(0,08,C[0][0]);  NumOut(30,08,C[0][1]);
  NumOut(0,00,C[1][0]);  NumOut(30,00,C[1][1]);

  while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();

  A[0][0]=1;   A[0][1]=1;
  A[1][0]=1;   A[1][1]=0;

  NumOut(0,56,A[0][0]);  NumOut(30,56,A[0][1]);
  NumOut(0,48,A[1][0]);  NumOut(30,48,A[1][1]);

  TextOut(0,40,"Rotate 30deg GEO");
  MatrixRotate2DGeo(A,B,30);
  NumOut(0,32,B[0][0]);  NumOut(60,32,B[0][1]);
  NumOut(0,24,B[1][0]);  NumOut(60,24,B[1][1]);

  TextOut(0,16,"Rotate 90deg GEO");
  MatrixRotate2DGeo(A,C,90);
  NumOut(0,08,C[0][0]);  NumOut(60,08,C[0][1]);
  NumOut(0,00,C[1][0]);  NumOut(60,00,C[1][1]);

   while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();

  while(1);

}
jhc and spillerrec tweaked version:

Code: Select all

            // Matrix algebra / arithmetics
// vers. 1.10t (HaWe+jch+splrc version)


#define MatrixMulS(_Out, _A, _s) asm { mul _Out, _A, _s }
#define MatrixDivS(_Out, _A, _s) asm { div _Out, _A, _s }

#define MatrixAdd(_Out, _A, _B) asm { add _Out, _A, _B }
#define MatrixSub(_Out, _A, _B) asm { sub _Out, _A, _B }

#define ArrayIndex(_out, _A, _i)   asm { index _out, _A, _i }
#define ArrayReplace(_A, _i, _new) asm { replace _A, _A, _i, _new }

#define BranchTest(_cmp, _lbl, _v1) asm{ brtst _cmp, _lbl, _v1 }
#define BranchComp(_cmp, _lbl, _v1, _v2) asm{ brcmp _cmp, _lbl, _v1, _v2 }


#define ArrayInit2D(array, tmp, init_val, dimx, dimy) { \
  ArrayInit(tmp, init_val, dimy);  \
  ArrayInit(array, tmp, dimx);     \
  ArrayInit(tmp,0,0);              \
}


void MatrixDiag(float A[][], float &D[][], int RS) {   // Diagonal matrix
   float tmp[], s;
   ArrayInit2D(D, tmp, 0, RS,RS);
   for (s=0; s<RS; s++) {
     D[s][s]=A[s][s];
   }
}


void MatrixTransp(float A[][], float &C[][], int R, int S) {
   float tmp[], arr_temp[], val_temp;
   int s, r;
   ArrayInit(tmp, 0, R);
   ArrayInit(C, tmp, S);
   s = S;
   lbl_Trans_start_s:
   {
      s--;
      r = R;
      lbl_Trans_start_r:
      {
         r--;
         ArrayIndex(arr_temp, A, r);
         ArrayIndex(val_temp, arr_temp, s);
         ArrayReplace(tmp, r, val_temp);
      }
      BranchTest(GT, lbl_Trans_start_r, r);
      ArrayReplace(C, s, tmp);
   }
   BranchTest(GT, lbl_Trans_start_s, s);
}


void VectorTransp(float A[], float &C[][], int R) {
   int r;
   float tmp[];
   ArrayInit2D(C,tmp, 0, 1,R);
   for (r=0; r<R; r++) {
     C[0][r]=A[r];
   }
}



                                               // N x M Matrix  *  M x K Matrix, NEW version
void MatrixMatrixMult(float A[][], float B[][], float &C[][], int N, int M, int K){
   float arr_temp[];
   //Set size of C
   ArrayInit( arr_temp, 0, K );
   ArrayInit( C, arr_temp, N );

lbl_Mult_start_i:   N--;   //Loop start
      //Speed up arrays
      float A_i[];
      float C_i[];
      ArrayIndex(A_i, A, N);
      ArrayIndex(C_i, C, N);

      int j = K;
lbl_Mult_start_j: j--;   //Second loop start
         float sum = 0;

         int s = M;
lbl_Mult_start_s: s--;   //Third loop start

            //sum += A_i[s]*B[s][j];
            float temp, sum_temp;
            ArrayIndex(sum_temp, A_i, s);
            ArrayIndex(arr_temp, B, s);
            ArrayIndex(temp, arr_temp, j);
            sum_temp *= temp;
            sum += sum_temp;
         asm { brtst GT, lbl_Mult_start_s, s }
         ArrayReplace(C_i, j, sum);
      asm { brtst GT, lbl_Mult_start_j, j }
      ArrayReplace(C, N, C_i);
     asm { brtst GT, lbl_Mult_start_i, N }
}


                                                // Matrix[n,m] x Vector[m]
void MatrixVectorMult(float A[][], float B[], float &C[], int N, int M){   
  int i, s;
  ArrayInit(C,0, M);
  for (i=0; i<M; ++i) {
     C[i]=0;
     for (s=0; s<N; ++s) {
       C[i]=C[i] + A[i][s]*B[s];
     }
  }
}

                                                 // Vector[n]^T x Matrix [n,m]
void VectorMatrixMult(float A[], float B[][], float &C[], int N, int M){    // K=1
  int j, s;
  ArrayInit(C,0, M);
  for (j=0; j<M; ++j) {
     C[j]=0;
     for (s=0; s<N; ++s) {
       C[j]=C[j] + A[s]*B[s][j];
     }
  }
}


void MatrixRotate2DMath(float A[][], float &C[][], float angleDeg) {
   float tmp[], x, B[][];

   ArrayInit2D(C, tmp, 0, 2, 2);

   x=cosd(angleDeg);
   ArrayInit2D(B, tmp, x, 2, 2);
   x=sind(angleDeg);
   B[0][1]=-x;
   B[1][0]= x;

   MatrixMatrixMult(B, A, C, 2, 2, 2);
}


void VectorRotate2DMath(float A[], float &C[], float angleDeg) {
   float tmp[], x, B[][];

   ArrayInit(C, 0, 2);

   x=cosd(angleDeg);
   ArrayInit2D(B, tmp, x, 2, 2);
   x=sind(angleDeg);
   B[0][1]=-x;
   B[1][0]= x;

   MatrixVectorMult(B, A, C, 2, 2);
}


void MatrixRotate2DGeo(float A[][], float &C[][], float angleDeg) {
   float tmp[], x, B[][];
   angleDeg*=-1;
   ArrayInit2D(C, tmp, 0, 2, 2);

   x=cosd(angleDeg);
   ArrayInit2D(B, tmp, x, 2, 2);
   x=sind(angleDeg);
   B[0][1]=-x;
   B[1][0]= x;

   MatrixMatrixMult(B, A, C, 2, 2, 2);
}


void VectorRotate2DGeo(float A[], float &C[], float angleDeg) {
   float tmp[], x, B[][];
   angleDeg*=-1;
   ArrayInit(C, 0, 2);

   x=cosd(angleDeg);
   ArrayInit2D(B, tmp, x, 2, 2);
   x=sind(angleDeg);
   B[0][1]=-x;
   B[1][0]= x;

   MatrixVectorMult(B, A, C, 2, 2);
}


float MatrixDet2x2(float A[][]) {               // Determinante 1x1, 2x2, 3x3
   float r0[], r1[], r2[], v00, v01, v02, v10, v11, v12, v20, v21, v22;

      r0 = A[0];
      r1 = A[1];
      return ( r0[0]*r1[1]- r0[1]*r1[0] );
}


float MatrixDet3x3(float A[][]) {               // Determinante  3x3
   float r0[], r1[], r2[], v00, v01, v02, v10, v11, v12, v20, v21, v22;

      r0 = A[0];
      r1 = A[1];
      r2 = A[2];
      v00 = r0[0]; v01 = r0[1]; v02 = r0[2];
      v10 = r1[0]; v11 = r1[1]; v12 = r1[2];
      v20 = r2[0]; v21 = r2[1]; v22 = r2[2];
      return (v00*v11*v22
             +v01*v12*v20
             +v02*v10*v21
             -v02*v11*v20
             -v01*v10*v22
             -v00*v12*v21);

}


void MatrixAdj2x2 (float A[][], float &C[][]) {  // Adjugate=Adjunkte 1x1, 2x2, 3x3
  float r0[], r1[], r2[], c0[], c1[], c2[], t0, t1, t2;

    ArrayIndex(r0, A, 0);
    ArrayIndex(r1, A, 1);
    ArrayIndex(t0, r1, 1);
    ArrayIndex(t1, r0, 1);
    asm { mul t1, t1, -1 }
    ArrayBuild(c0, t0, t1);
    ArrayIndex(t0, r1, 0);
    asm { mul t0, t0, -1 }
    ArrayIndex(t1, r0, 0);
    ArrayBuild(c1, t0, t1);
    ArrayBuild(C, c0, c1);

}


void MatrixAdj3x3 (float A[][], float &C[][]) {  // Adjugate=Adjunkte 1x1, 2x2, 3x3
  float r0[], r1[], r2[], c0[], c1[], c2[], t0, t1, t2;


    float v00, v01, v02, v10, v11, v12, v20, v21, v22;
    r0 = A[0]; r1 = A[1]; r2 = A[2];
    v00 = r0[0]; v01 = r0[1]; v02 = r0[2];
    v10 = r1[0]; v11 = r1[1]; v12 = r1[2];
    v20 = r2[0]; v21 = r2[1]; v22 = r2[2];

    t0 = v11*v22-v12*v21;
    t1 = v02*v21-v01*v22;
    t2 = v01*v12-v02*v11;
    ArrayBuild(c0, t0, t1, t2);

    t0 = v12*v20-v10*v22;
    t1 = v00*v22-v02*v20;
    t2 = v02*v10-v00*v12;
    ArrayBuild(c1, t0, t1, t2);

    t0 = v10*v21-v11*v20;
    t1 = v01*v20-v00*v21;
    t2 = v00*v11-v01*v10;
    ArrayBuild(c2, t0, t1, t2);

    ArrayBuild(C, c0, c1, c2);

}


bool MatrixInverse2x2(float A[][], float &C[][]) {
   float det;

   det=MatrixDet2x2(A);
   if (det==0) return 0;
   else {
     MatrixAdj2x2(A, C);
     C/=det;
     return 1;
   }
}


bool MatrixInverse3x3(float A[][], float &C[][]) {
   float det;

   det=MatrixDet3x3(A);
   if (det==0) return 0;
   else {
     MatrixAdj3x3(A, C);
     C/=det;
     return 1;
   }
}



void MatrixLinear(float A[][], float &C[], int R, int S) {
  int i=0, s, r;
  int dim=R*S;
  ArrayInit(C, 0, dim);
  for (s=0; s<S; ++s) {
    for (r=0; r<R; ++r) {
      C[i++]=A[r][s];
    }
  }
}


void SubMatrix(float A[][], float &C[][], int RS, int r, int s) {
  int i=0, j=0, x=0, y=0, dim_1;
  float tmp[];

  dim_1=RS-1;
  ArrayInit2D(C, tmp, 0, dim_1, dim_1);

  for (y=0; y<RS; ++y) {
    for (x=0; x<RS; ++x) {
      if ((x!=r)&&(y!=s))   {
         C[i][j]=A[x][y];
         ++i;
         if (i>=dim_1){
           i=0; ++j;
         }
      }
    }
  }
}


float MatrixMinor(float A[][], int RS, int r, int s) {
   float C[][];

   SubMatrix(A, C, RS, r, s);

   if(RS==3) {
     return MatrixDet2x2(C);
   }
   else
   if(RS==4) {
     return MatrixDet3x3(C);
   }
}


float MatrixCofactor(float A[][], int RS, int r, int s) {
   float x;

   return pow(-1, r+s)* MatrixMinor(A, RS, r, s);;
}


float MatrixDet4x4(float A[][])  {
   float det=0, x=0;
   int i, j=0, n0, max0=0, imax0, jmax0=0;

   for (j=0; j<4; ++j) {    // find column with most 0's: jmax0
     n0=0;
     for (i=0; i<4; ++i) {
       if (A[i][j]==0) ++n0;
     }
     if (max0<n0) {max0=n0; jmax0=j;}
   }

   for (i=0; i<4; ++i) {    // find row with most 0's: imax0
     n0=0;
     for (j=0; j<4; ++j) {
       if (A[i][j]==0) ++n0;
     }
     if (max0<n0) {max0=n0; imax0=i;}
   }


   if (jmax0>imax0) {
     for (i=0; i<4; ++i) {         // develop from jmax0
       if ( A[i][jmax0]!=0) {      // 0*?=0
          x=A[i][jmax0]*MatrixCofactor(A, 4, i, jmax0);
          det+=x;
       }
     }
     //TextOut(72,56,"j"); NumOut(80,56,jmax0); //debug
     return det;
   }
   else  {
     for (j=0; j<4; ++j) {         // develop from imax0
       if ( A[imax0][j]!=0) {      // 0*?=0
          x=A[imax0][j]*MatrixCofactor(A, 4, imax0, j);
          det+=x;
       }
     }
     //TextOut(72,56,"i"); NumOut(80,56,imax0); //debug
     return det;
   }
}


task main() {
  float A[2][2], B[2][2], C[2][2], x;
  float O[4][4], T[][], L[][];
  

  O[0][0]=0;   O[0][1]=0;   O[0][2]=8;   O[0][3]=22;
  O[1][0]=1;   O[1][1]=5;   O[1][2]=9;   O[1][3]=13;
  O[2][0]=2;   O[2][1]=6;   O[2][2]=10;  O[2][3]=14;
  O[3][0]=3;   O[3][1]=7;   O[3][2]=22;  O[3][3]=15;

  NumOut(0,56,O[0][0]);  NumOut(18,56,O[0][1]);  NumOut(36,56,O[0][2]);  NumOut(54,56,O[0][3]);
  NumOut(0,48,O[1][0]);  NumOut(18,48,O[1][1]);  NumOut(36,48,O[1][2]);  NumOut(54,48,O[1][3]);
  NumOut(0,40,O[2][0]);  NumOut(18,40,O[2][1]);  NumOut(36,40,O[2][2]);  NumOut(54,40,O[2][3]);
  NumOut(0,32,O[3][0]);  NumOut(18,32,O[3][1]);  NumOut(36,32,O[3][2]);  NumOut(54,32,O[3][3]);
                                                 TextOut(72,48,"det");
  x=MatrixDet4x4(O);                             NumOut(72,40,x);        /*  968   */

  SubMatrix(O,T,4, 2,1);  // expanded to A[i][j]
  TextOut(0,24,"SubMatrix[2;1]");
  NumOut(0,16,T[0][0]);  NumOut(24,16,T[0][1]);  NumOut(48,16,T[0][2]);
  NumOut(0,08,T[1][0]);  NumOut(24,08,T[1][1]);  NumOut(48, 8,T[1][2]);
  NumOut(0, 0,T[2][0]);  NumOut(24, 0,T[2][1]);  NumOut(48, 0,T[2][2]);

  while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();

  NumOut(0,56,T[0][0]);  NumOut(24,56,T[0][1]);  NumOut(48,56,T[0][2]);   TextOut(72,56,"det");
  NumOut(0,48,T[1][0]);  NumOut(24,48,T[1][1]);  NumOut(48,48,T[1][2]);   NumOut (72,48, MatrixDet3x3(T));
  NumOut(0,40,T[2][0]);  NumOut(24,40,T[2][1]);  NumOut(48,40,T[2][2]);   /*  82   */

  SubMatrix(T,C,3, 0,1);
  TextOut(0,32,"SubMatrix[0;1]");
  NumOut(0,24,C[0][0]);  NumOut(24,24,C[0][1]);    /* 1   13 */           TextOut(48,24,"det");
  NumOut(0,16,C[1][0]);  NumOut(24,16,C[1][1]);    /* 3   15 */           x=MatrixDet2x2(C);
                                                                          NumOut (72,24, x);
                                                                          /* 15-(3*13)= -24     */

  TextOut(0,8,"MMinor[0;1]");                                             /* TEST: */
  x=MatrixMinor(T, 3, 0,1);                       NumOut (72, 8, x);      /* Minor T[0,1]= det(C)= -24  */
  TextOut(0,0,"MCofac[0;1]");
  x=MatrixCofactor(T, 3, 0,1);                    NumOut (72, 0, x);      /* Cof T[0,1]= det(C)*(-1)^(0+1) = +24 */



  while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();


  A[0][0]=1;   A[0][1]=3;
  A[1][0]=2;   A[1][1]=4;
  NumOut(0,56,A[0][0]);  NumOut(30,56,A[0][1]);
  NumOut(0,48,A[1][0]);  NumOut(30,48,A[1][1]);

  TextOut(0,40,"B=A*10");
  x=10;
  MatrixMulS(B, A, x);
//  B=A; B*=x;
  NumOut(0,32,B[0][0]);  NumOut(30,32,B[0][1]);
  NumOut(0,24,B[1][0]);  NumOut(30,24,B[1][1]);

  TextOut(0,16,"C=A+B");
  MatrixAdd(C, A, B);
//  C=A; C+=B;
  NumOut(0,08,C[0][0]);  NumOut(30,08,C[0][1]);
  NumOut(0,00,C[1][0]);  NumOut(30,00,C[1][1]);


  while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();

  NumOut(0,56,A[0][0]);  NumOut(30,56,A[0][1]);
  NumOut(0,48,A[1][0]);  NumOut(30,48,A[1][1]);

  TextOut(0,40,"Adjunkte");
  MatrixAdj2x2(A,C);
  NumOut(0,32,C[0][0]);  NumOut(30,32,C[0][1]);
  NumOut(0,24,C[1][0]);  NumOut(30,24,C[1][1]);

  TextOut(0,16,"Inverse");
  MatrixInverse2x2(A,C);
  NumOut(0,08,C[0][0]);  NumOut(30,08,C[0][1]);
  NumOut(0,00,C[1][0]);  NumOut(30,00,C[1][1]);

  while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();

  NumOut(0,56,A[0][0]);  NumOut(30,56,A[0][1]);
  NumOut(0,48,A[1][0]);  NumOut(30,48,A[1][1]);

  TextOut(0,40,"Inverse");
  MatrixInverse2x2(A,B);
  NumOut(0,32,B[0][0]);  NumOut(30,32,B[0][1]);
  NumOut(0,24,B[1][0]);  NumOut(30,24,B[1][1]);

  TextOut(0,16,"Matrix * Inverse");
  MatrixMatrixMult(A,B,C,2,2,2);
  NumOut(0,08,C[0][0]);  NumOut(30,08,C[0][1]);
  NumOut(0,00,C[1][0]);  NumOut(30,00,C[1][1]);

  while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();
  
  A[0][0]=1;   A[0][1]=1;
  A[1][0]=1;   A[1][1]=0;

  NumOut(0,56,A[0][0]);  NumOut(30,56,A[0][1]);
  NumOut(0,48,A[1][0]);  NumOut(30,48,A[1][1]);

  TextOut(0,40,"Rotate 30deg GEO");
  MatrixRotate2DGeo(A,B,30);
  NumOut(0,32,B[0][0]);  NumOut(60,32,B[0][1]);
  NumOut(0,24,B[1][0]);  NumOut(60,24,B[1][1]);

  TextOut(0,16,"Rotate 90deg GEO");
  MatrixRotate2DGeo(A,C,90);
  NumOut(0,08,C[0][0]);  NumOut(60,08,C[0][1]);
  NumOut(0,00,C[1][0]);  NumOut(60,00,C[1][1]);
  
   while (!ButtonPressed(BTNCENTER, false));
  while ( ButtonPressed(BTNCENTER, false));
  ClearScreen();

  while(1);

}

Re: NXC: resolved - Matrix algebra library

Posted: 11 Jul 2011, 21:25
by spillerrec
The firmware is polymorphic, all the basic arithmetical operations also works on arrays and structures. If you are willing to drop the ranges, you should be able to do this:

Code: Select all

void MatrixScalarProduct (float A[][], float scalar, float &C[][]) {
  C = A;
  A *= scalar;
}
I haven't tested it, but it should work. You need to use the operators: +=, -=, *= and /= for it to work at the moment as you can't do "C = A * scalar;" currently. (If you really want to do it in one operation, do "asm{ mul C, A, scalar }".)
This will be much much faster than anything you can do in NXC. I used it to optimize my Bézier curve library and it is amazing to see how little the speed is affected when the array length is doubled.

If you really want the ranges you can use some of the array functions to pull out some elements of the array. It is going to be ugly since you are using multidimensional arrays... I wish there where better support for multidimensional arrays in the firmware array functions, I will add it to the wish list one of the following days.

Re: NXC: resolved - Matrix algebra library

Posted: 12 Jul 2011, 08:19
by HaWe
thx Spiller for the speed enhancement!
Surely I can drop the ranges for matrix scalar product and addition (as the result has the same dimensions as the inputs), but of course it won't go for matrix multiplication (as if the 1st matrix is n x m, the 2nd matrix must be m x k, and the result is n x k).
The Determinate of course is size-sensitive, but for the Adjunkte and the Inverse it's again possible because all of them are square matrices by the same sizes like the input sizes.

My first goal was - as usual - just to have some instruments to work with - speed comes 2nd, but is appreciated.

As you probably saw, Product, Multiplication, and Addition are generalized,
but Determinante, Adjunkte, and Inverse are specific to 2x2 and 3x3 matrices. (sry, don't know the English idioms)
Other enhancements maybe will be to have also these to general n x n square matrices (e.g. the Determinante by the Kramersche Regel) - Adjunkte and Inverse will not be so easy to do.

But now I'm glad that already my first step is done, and may calculate the inverse matrix to a any given (invertible) 2x2 or 3x3 matrix.

If you want to help to have the latter algorithms also for 4x4 and 5x5 or general n x n I'd be glad if you could show me!

Re: NXC: resolved - Matrix algebra library

Posted: 12 Jul 2011, 11:18
by HaWe
ps
You need to use the operators: +=, -=, *= and /= for it to work at the moment as you can't do "C = A * scalar;" current
so how should I do then C=A+B? for all A[r], B[r], C[r]

maybe
A+=B; C=A;
?
(that's not a real problem for the moment though, but as I said, speed is appreciated^^)

Re: NXC: resolved - Matrix algebra library

Posted: 12 Jul 2011, 13:36
by spillerrec
You should do:

Code: Select all

C = A;
A += B;
I will try to optimize your existing functions. I will see what I can do with Matrix multiplication. For the functions that do a lot of static array lookups I will cache those results, it should give a decent speedup.
If I have time then perhaps I will brush up my Math and try to add some of the missing functions. No promises though.

Just a tip, if you want to know the English versions of those kind of words then try to lookup the German word on Wikipedia. If this success, just press "English" in the language toolbar. It is not a failsafe approach but it works rather well.

Re: NXC: resolved - Matrix algebra library

Posted: 12 Jul 2011, 13:49
by HaWe
why this:

Code: Select all

C = A;
A += B;
A and B are the origin matrices, C is the one which is to be returned.
A and B are passed by value, C by reference.

So my suggesstion was
A += B; // adds B to A only internally, so no change of the origin
C = A; // assignes A to C, returns C by reference
- or -
C = A; // assignes A to C
C +=B; // adds B to C, returns C by reference

the same wrt scalar product:

A *= scalar; // multiplies A by a scalar only internally
C = A; // assignes A to C, returns C by reference
- or -
C = A; // assignes A to C
C *= scalar; // multiplies C by a scalar, returns C

or am I missing something?
But the main thing to me is that is works (it indeed does), speed is only secondary.

edit:
I applied the changes to the code above, it works now with the latter changes (see there!)

C = A; // assignes A to C
C *= scalar; // multiplies C by a scalar, returns C

C = A; // assignes A to C
C +=B; // adds B to C, returns C by reference

But as one can do addition and scalar product actually directly, it's of course redundant now.
So the rest is left for optimizing! :)

thx for contributing!

Re: NXC: resolved - Matrix algebra library

Posted: 12 Jul 2011, 14:58
by spillerrec
Yes, they both work. However lets say you want to do further optimization.
We currently have a function with two lines of code. When we call MatrixScalarProduct( A, scalar, C ) it will look like this: (pseudo code)

Code: Select all

temp_A = A;
temp_scalar = scalar;
temp_C = C;
MatrixScalarProduct();
C = temp_C;
5 lines of code to do 2 lines. Inlining doesn't help much since it just replaces the function call with the code, it doesn't remove the constant temporary variables.

The most efficient in this case is to use a Macro function. This is where the two versions begins to differ, if we do "A+=B; C=A;" A will be overwritten, we would have to store it in a temporary variable first. This is not required with "C = A; C += B;". This lowers memory usage and requires fewer instructions.
Macro functions are not type checked but since the firmware is polymorphic this actually doesn't matter that much. It does matter some though, "C=A;" casts A to the type of C! So doing it in one instruction is better:

Code: Select all

//navite Matrix Scalar operations, no type checking!
#define MatrixScalarAdd( A, scalar, out ) asm{ add out, A, scalar }
#define MatrixScalarSubstract( A, scalar, out ) asm{ sub out, A, scalar }
#define MatrixScalarProduct( A, scalar, out ) asm{ mul out, A, scalar }
#define MatrixScalarDivision( A, scalar, out ) asm{ div out, A, scalar }

//native Matrix Matrix operations, no type checking!
#define MatrixMatrixAdd( A, B, out ) asm{ add out, A, B }
#define MatrixMatrixSubstract( A, B, out ) asm{ sub out, A, B }
I haven't tested these properly though.


I have looked into Matrix multiplication and it will be hard to do this efficiently without array iteration. It could perhaps be possible it there was an efficient way to get the transpose matrix but I don't think this will be possible.

Re: NXC: resolved - Matrix algebra library

Posted: 12 Jul 2011, 15:17
by HaWe
yes, and as I wrote already above:
But as one can do addition and scalar product actually directly, it's of course redundant now.
so we can drop matrix*scalar and matrix+matrix because we can use it in our code directly, also division and substraction.
A general determinant(e) (not just one for 2x2 and one for 3x3) would be fine to have, too, but the algorithms for that (Laplace and Gauss) are both a big chunk of code. Even worse it's for calculating a "general matrix adjugate/Adjunkte".
And then worst of all is to use both for a "general inverse".

Re: NXC: resolved - Matrix algebra library

Posted: 12 Jul 2011, 21:24
by linusa
spillerrec wrote:I have looked into Matrix multiplication and it will be hard to do this efficiently without array iteration. It could perhaps be possible it there was an efficient way to get the transpose matrix but I don't think this will be possible.
Getting the transpose matrix is very efficient. Instead of accessing element [j], you access [j]. This operation is almost "for free" in popular linear algebra libraries or software, such as e.g. OpenCV or MATLAB. I haven't checked the exact sourcecode (you could do that for OpenCV's function cv::mat::t() ), but I would implement it like this:
- Store matrix elements in a 2D array
- Store a flag whether the matrix is transposed or not
- In the element accessor getElement(int i, int j) (or operator[]), you check the transposed flag and swap i, j or not. For algorithms, you can have two copy-pasted versions with i, j swapped.
This effectively makes the transpose-operation come down to 1 additional if-statement.

The general problem of efficient matrix multiplication is not solved yet, see http://en.wikipedia.org/wiki/Matrix_mul ... iplication

Re: NXC: resolved - Matrix algebra library

Posted: 13 Jul 2011, 08:12
by HaWe
BTW, transposed matrices are not an issue so far - it's about determinant, adjugate, and inverse.
All times are UTC
Page 1 of 6

Powered by phpBB® Forum Software © phpBB Limited