Matrix.cpp

00001 #include <stdio.h>
00002 #include "Matrix.h"
00003 
00004 unsigned int matrix_error = 0;
00005 
00006 void Vector_Cross_Product(float C[3], float A[3], float B[3])
00007 {
00008     C[0] = (A[1] * B[2]) - (A[2] * B[1]);
00009     C[1] = (A[2] * B[0]) - (A[0] * B[2]);
00010     C[2] = (A[0] * B[1]) - (A[1] * B[0]);
00011   
00012     return;
00013 }
00014 
00015 void Vector_Scale(float C[3], float A[3], float b)
00016 {
00017     for (int m = 0; m < 3; m++)
00018         C[m] = A[m] * b;
00019         
00020     return;
00021 }
00022 
00023 float Vector_Dot_Product(float A[3], float B[3])
00024 {
00025     float result = 0.0;
00026 
00027     for (int i = 0; i < 3; i++) {
00028         result += A[i] * B[i];
00029     }
00030     
00031     return result;
00032 }
00033 
00034 void Vector_Add(float C[3], float A[3], float B[3])
00035 {
00036     for (int m = 0; m < 3; m++)
00037         C[m] = A[m] + B[m];
00038         
00039     return;
00040 }
00041 
00042 void Vector_Add(float C[3][3], float A[3][3], float B[3][3])
00043 {
00044     for (int m = 0; m < 3; m++)
00045         for (int n = 0; n < 3; n++)
00046             C[m][n] = A[m][n] + B[m][n];
00047 }
00048 
00049 void Matrix_Add(float C[3][3], float A[3][3], float B[3][3])
00050 {
00051     for (int i = 0; i < 3; i++) {
00052         for (int j = 0; j < 3; j++) {
00053            C[i][j] = A[i][j] + B[i][j];
00054         }
00055     }
00056 }
00057 
00058 void Matrix_Add(int n, int m, float *C, float *A, float *B)
00059 {
00060     for (int i = 0; i < n*m; i++) {
00061        C[i] = A[i] + B[i];
00062     }
00063 }
00064 
00065 void Matrix_Subtract(int n, int m, float *C, float *A, float *B)
00066 {
00067     for (int i = 0; i < n*m; i++) {
00068        C[i] = A[i] - B[i];
00069     }
00070 }
00071 
00072 
00073 
00074 // grabbed from MatrixMath library for Arduino
00075 // http://arduino.cc/playground/Code/MatrixMath
00076 // E.g., the equivalent Octave script:
00077 //   A=[x; y; z];
00078 //   B=[xx xy xz; yx yy yz; zx xy zz]; 
00079 //   C=A*B;
00080 // Would be called like this:
00081 //   Matrix_Mulitply(1, 3, 3, C, A, B);
00082 //
00083 void Matrix_Multiply(int m, int p, int n, float *C, float *A, float *B)
00084 {
00085     // A = input matrix (m x p)
00086     // B = input matrix (p x n)
00087     // m = number of rows in A
00088     // p = number of columns in A = number of rows in B
00089     // n = number of columns in B
00090     // C = output matrix = A*B (m x n)
00091     for (int i=0; i < m; i++) {
00092         for(int j=0; j < n; j++) {
00093             C[n*i+j] = 0;
00094             for (int k=0; k < p; k++) {
00095                 C[i*n+j] += A[i*p+k] * B[k*n+j];
00096             }
00097         }
00098     }
00099         
00100     return;
00101 }
00102 
00103 void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3])
00104 {
00105     for (int i = 0; i < 3; i++) {
00106         for (int j = 0; j < 3; j++) {
00107             C[i][j] = 0;
00108             for (int k = 0; k < 3; k++) {
00109                C[i][j] += A[i][k] * B[k][j];
00110             }
00111         }
00112     }
00113 }
00114 
00115 
00116 void Matrix_Transpose(int n, int m, float *C, float *A)
00117 {
00118     for (int i=0; i < n; i++) {
00119         for (int j=0; j < m; j++) {
00120             C[j*n+i] = A[i*m+j];
00121         }
00122     }
00123 }
00124 
00125 #define fabs(x) (((x) < 0) ? -x : x)
00126 
00127 // grabbed from MatrixMath library for Arduino
00128 // http://arduino.cc/playground/Code/MatrixMath
00129 //Matrix Inversion Routine
00130 // * This function inverts a matrix based on the Gauss Jordan method.
00131 // * Specifically, it uses partial pivoting to improve numeric stability.
00132 // * The algorithm is drawn from those presented in 
00133 //     NUMERICAL RECIPES: The Art of Scientific Computing.
00134 // * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED
00135 void Matrix_Inverse(int n, float *A)
00136 {
00137     // A = input matrix AND result matrix
00138     // n = number of rows = number of columns in A (n x n)
00139     int pivrow=0;   // keeps track of current pivot row
00140     int k,i,j;        // k: overall index along diagonal; i: row index; j: col index
00141     int pivrows[n]; // keeps track of rows swaps to undo at end
00142     float tmp;        // used for finding max value and making column swaps
00143     
00144     for (k = 0; k < n; k++) {
00145         // find pivot row, the row with biggest entry in current column
00146         tmp = 0;
00147         for (i = k; i < n; i++) {
00148             if (fabs(A[i*n+k]) >= tmp) {    // 'Avoid using other functions inside abs()?'
00149                 tmp = fabs(A[i*n+k]);
00150                 pivrow = i;
00151             }
00152         }
00153         
00154         // check for singular matrix
00155         if (A[pivrow*n+k] == 0.0f) {
00156             matrix_error |= SINGULAR_MATRIX;
00157             //fprintf(stdout, "Inversion failed due to singular matrix");
00158             return;
00159         }
00160         
00161         // Execute pivot (row swap) if needed
00162         if (pivrow != k) {
00163             // swap row k with pivrow
00164             for (j = 0; j < n; j++) {
00165                 tmp = A[k*n+j];
00166                 A[k*n+j] = A[pivrow*n+j];
00167                 A[pivrow*n+j] = tmp;
00168             }
00169         }
00170         pivrows[k] = pivrow;    // record row swap (even if no swap happened)
00171         
00172         tmp = 1.0f/A[k*n+k];    // invert pivot element
00173         A[k*n+k] = 1.0f;        // This element of input matrix becomes result matrix
00174         
00175         // Perform row reduction (divide every element by pivot)
00176         for (j = 0; j < n; j++) {
00177             A[k*n+j] = A[k*n+j]*tmp;
00178         }
00179         
00180         // Now eliminate all other entries in this column
00181         for (i = 0; i < n; i++) {
00182             if (i != k) {
00183                 tmp = A[i*n+k];
00184                 A[i*n+k] = 0.0f;  // The other place where in matrix becomes result mat
00185                 for (j = 0; j < n; j++) {
00186                     A[i*n+j] = A[i*n+j] - A[k*n+j]*tmp;
00187                 }
00188             }
00189         }
00190     }
00191     
00192     // Done, now need to undo pivot row swaps by doing column swaps in reverse order
00193     for (k = n-1; k >= 0; k--) {
00194         if (pivrows[k] != k) {
00195             for (i = 0; i < n; i++) {
00196                 tmp = A[i*n+k];
00197                 A[i*n+k] = A[i*n+pivrows[k]];
00198                 A[i*n+pivrows[k]] = tmp;
00199             }
00200         }
00201     }
00202     return;
00203 }
00204 
00205 
00206 void Matrix_Copy(int n, int m, float *C, float *A)
00207 {
00208     for (int i=0; i < n*m; i++)
00209         C[i] = A[i];
00210 }
00211 
00212 void Matrix_print(int n, int m, float *A, const char *name)
00213 {
00214     fprintf(stdout, "%s=[", name);
00215     for (int i=0; i < n; i++) {
00216         for (int j=0; j < m; j++) {
00217             fprintf(stdout, "%5.5f", A[i*m+j]);
00218             if (j < m-1) fprintf(stdout, ", ");
00219         }
00220         if (i < n-1) fprintf(stdout, "; ");
00221     }
00222     fprintf(stdout, "]\n");
00223 }  
00224 
00225 
00226 void Vector_Print(float A[3], const char *name)
00227 {
00228     fprintf(stdout, "%s=[ ", name);
00229     for (int i=0; i < 3; i++)
00230         fprintf(stdout, "%5.5f ", A[i]);
00231     fprintf(stdout, "]\n");
00232     
00233     return;
00234 }
00235