Permalink
Browse files

adding what we have so far

  • Loading branch information...
1 parent 514005c commit da4ec82e5b4c134ebfab9b977e0476103b2fe650 @zobot zobot committed Feb 22, 2012
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,70 @@
+/*
+ * MatrixMathExample.pde
+ *
+ * Example code for Matrix Math utilities
+ *
+ * Created by Charlie Matlack on 12/18/10. Changed name.
+ * Original code by RobH45345 on Arduino Forums, taken from unknown source.
+ *
+ */
+
+#include <MatrixMath.h>
+
+#define N (5)
+MatrixMath math;
+
+float A[N][N];
+float B[N][N];
+float C[N][N];
+float max = 10; // maximum random matrix entry range
+
+void setup() {
+ Serial.begin(9600);
+ // Initialize A to a random matrix and B to the identity
+ for (int i = 0; i < N; i++)
+ {
+ for (int j = 0; j < N; j++)
+ {
+ A[i][j] = random(max) - max/2.0f;
+ if (i == j)
+ {
+ B[i][j] = 1.0f;
+ } else
+ {
+ B[i][j] = 0.0f;
+ }
+ }
+ }
+
+}
+
+void loop(){
+ math.MatrixMult((float*)A,(float*)B,N,N,N,(float*)C);
+
+ Serial.println("\nInitial values after C = A*B:");
+ math.MatrixPrint((float*)A,N,N,"A");
+ math.MatrixPrint((float*)B,N,N,"B");
+ math.MatrixPrint((float*)C,N,N,"C");
+
+ math.MatrixCopy((float*)A,N,N,(float*)B);
+
+ Serial.println("\nCopied A to B:");
+ math.MatrixPrint((float*)A,N,N,"A");
+ math.MatrixPrint((float*)B,N,N,"B");
+ math.MatrixPrint((float*)C,N,N,"C");
+
+ math.MatrixInvert((float*)A,N);
+
+ Serial.println("\nInverted A:");
+ math.MatrixPrint((float*)A,N,N,"A");
+ math.MatrixPrint((float*)B,N,N,"B");
+ math.MatrixPrint((float*)C,N,N,"C");
+
+ math.MatrixMult((float*)A,(float*)B,N,N,N,(float*)C);
+
+ Serial.println("\nC = A*B");
+ math.MatrixPrint((float*)A,N,N,"A");
+ math.MatrixPrint((float*)B,N,N,"B");
+ math.MatrixPrint((float*)C,N,N,"C");
+while(1);
+}
@@ -0,0 +1,286 @@
+/*
+ * MatrixMath.cpp Library for MatrixMath
+ *
+ * Created by Charlie Matlack on 12/18/10.
+ * Modified from code by RobH45345 on Arduino Forums, taken from unknown source.
+ * MatrixMath.cpp
+ */
+
+#include "WProgram.h"
+#include "MatrixMath.h"
+
+#define NR_END 1
+
+MatrixMath::MatrixMath()
+{
+}
+
+// Matrix Printing Routine
+// Uses tabs to separate numbers under assumption printed float width won't cause problems
+void MatrixMath::MatrixPrint(float* A, int m, int n, String label){
+ // A = input matrix (m x n)
+ int i,j;
+ Serial.println();
+ Serial.println(label);
+ for (i=0; i<m; i++){
+ for (j=0;j<n;j++){
+ Serial.print(A[n*i+j]);
+ Serial.print("\t");
+ }
+ Serial.println();
+ }
+}
+
+void MatrixMath::MatrixCopy(float* A, int n, int m, float* B)
+{
+ int i, j, k;
+ for (i=0;i<m;i++)
+ for(j=0;j<n;j++)
+ {
+ B[n*i+j] = A[n*i+j];
+ }
+}
+
+//Matrix Multiplication Routine
+// C = A*B
+void MatrixMath::MatrixMult(float* A, float* B, int m, int p, int n, float* C)
+{
+ // A = input matrix (m x p)
+ // B = input matrix (p x n)
+ // m = number of rows in A
+ // p = number of columns in A = number of rows in B
+ // n = number of columns in B
+ // C = output matrix = A*B (m x n)
+ int i, j, k;
+ for (i=0;i<m;i++)
+ for(j=0;j<n;j++)
+ {
+ C[n*i+j]=0;
+ for (k=0;k<p;k++)
+ C[n*i+j]= C[n*i+j]+A[p*i+k]*B[n*k+j];
+ }
+}
+
+
+//Matrix Addition Routine
+void MatrixMath::MatrixAdd(float* A, float* B, int m, int n, float* C)
+{
+ // A = input matrix (m x n)
+ // B = input matrix (m x n)
+ // m = number of rows in A = number of rows in B
+ // n = number of columns in A = number of columns in B
+ // C = output matrix = A+B (m x n)
+ int i, j;
+ for (i=0;i<m;i++)
+ for(j=0;j<n;j++)
+ C[n*i+j]=A[n*i+j]+B[n*i+j];
+}
+
+
+//Matrix Subtraction Routine
+void MatrixMath::MatrixSubtract(float* A, float* B, int m, int n, float* C)
+{
+ // A = input matrix (m x n)
+ // B = input matrix (m x n)
+ // m = number of rows in A = number of rows in B
+ // n = number of columns in A = number of columns in B
+ // C = output matrix = A-B (m x n)
+ int i, j;
+ for (i=0;i<m;i++)
+ for(j=0;j<n;j++)
+ C[n*i+j]=A[n*i+j]-B[n*i+j];
+}
+
+
+//Matrix Transpose Routine
+void MatrixMath::MatrixTranspose(float* A, int m, int n, float* C)
+{
+ // A = input matrix (m x n)
+ // m = number of rows in A
+ // n = number of columns in A
+ // C = output matrix = the transpose of A (n x m)
+ int i, j;
+ for (i=0;i<m;i++)
+ for(j=0;j<n;j++)
+ C[m*j+i]=A[n*i+j];
+}
+
+
+//Matrix Inversion Routine
+// * This function inverts a matrix based on the Gauss Jordan method.
+// * Specifically, it uses partial pivoting to improve numeric stability.
+// * The algorithm is drawn from those presented in
+// NUMERICAL RECIPES: The Art of Scientific Computing.
+// * The function returns 1 on success, 0 on failure.
+// * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED
+int MatrixMath::MatrixInvert(float* A, int n)
+{
+ // A = input matrix AND result matrix
+ // n = number of rows = number of columns in A (n x n)
+ int pivrow; // keeps track of current pivot row
+ int k,i,j; // k: overall index along diagonal; i: row index; j: col index
+ int pivrows[n]; // keeps track of rows swaps to undo at end
+ float tmp; // used for finding max value and making column swaps
+
+ for (k = 0; k < n; k++)
+ {
+ // find pivot row, the row with biggest entry in current column
+ tmp = 0;
+ for (i = k; i < n; i++)
+ {
+ if (abs(A[i*n+k]) >= tmp) // 'Avoid using other functions inside abs()?'
+ {
+ tmp = abs(A[i*n+k]);
+ pivrow = i;
+ }
+ }
+
+ // check for singular matrix
+ if (A[pivrow*n+k] == 0.0f)
+ {
+ Serial.println("Inversion failed due to singular matrix");
+ return 0;
+ }
+
+ // Execute pivot (row swap) if needed
+ if (pivrow != k)
+ {
+ // swap row k with pivrow
+ for (j = 0; j < n; j++)
+ {
+ tmp = A[k*n+j];
+ A[k*n+j] = A[pivrow*n+j];
+ A[pivrow*n+j] = tmp;
+ }
+ }
+ pivrows[k] = pivrow; // record row swap (even if no swap happened)
+
+ tmp = 1.0f/A[k*n+k]; // invert pivot element
+ A[k*n+k] = 1.0f; // This element of input matrix becomes result matrix
+
+ // Perform row reduction (divide every element by pivot)
+ for (j = 0; j < n; j++)
+ {
+ A[k*n+j] = A[k*n+j]*tmp;
+ }
+
+ // Now eliminate all other entries in this column
+ for (i = 0; i < n; i++)
+ {
+ if (i != k)
+ {
+ tmp = A[i*n+k];
+ A[i*n+k] = 0.0f; // The other place where in matrix becomes result mat
+ for (j = 0; j < n; j++)
+ {
+ A[i*n+j] = A[i*n+j] - A[k*n+j]*tmp;
+ }
+ }
+ }
+ }
+
+ // Done, now need to undo pivot row swaps by doing column swaps in reverse order
+ for (k = n-1; k >= 0; k--)
+ {
+ if (pivrows[k] != k)
+ {
+ for (i = 0; i < n; i++)
+ {
+ tmp = A[i*n+k];
+ A[i*n+k] = A[i*n+pivrows[k]];
+ A[i*n+pivrows[k]] = tmp;
+ }
+ }
+ }
+ return 1;
+}
+
+//Linear Solver Routine
+// * This function solves a linear equation based on the Gauss Jordan method.
+// * Specifically, it uses partial pivoting to improve numeric stability.
+// * The function returns 1 on success, 0 on failure.
+// * NOTE: The input matrix is REPLACED
+int LinearSolve(float* A, float* x, float* y, int n)
+{
+ // Solves the invertible system A x = y
+ // A = input matrix modified in place
+ // x = output x
+ // y = input y
+ // n = number of rows = number of columns in A (n x n)
+ int pivrow; // keeps track of current pivot row
+ int k,i,j; // k: overall index along diagonal; i: row index; j: col index
+ int pivrows[n]; // keeps track of rows swaps to undo at end
+ float tmp; // used for finding max value and making column swaps
+
+ for (k = 0; k < n; k++)
+ {
+ // find pivot row, the row with biggest entry in current column
+ tmp = 0;
+ for (i = k; i < n; i++)
+ {
+ if (abs(A[i*n+k]) >= tmp) // 'Avoid using other functions inside abs()?'
+ {
+ tmp = abs(A[i*n+k]);
+ pivrow = i;
+ }
+ }
+
+ // check for singular matrix
+ if (A[pivrow*n+k] == 0.0f)
+ {
+ Serial.println("Solving failed due to singular matrix");
+ return 0;
+ }
+
+ // Execute pivot (row swap) if needed
+ if (pivrow != k)
+ {
+ // swap row k with pivrow
+ for (j = 0; j < n; j++)
+ {
+ tmp = A[k*n+j];
+ A[k*n+j] = A[pivrow*n+j];
+ A[pivrow*n+j] = tmp;
+ }
+ }
+ pivrows[k] = pivrow; // record row swap (even if no swap happened)
+
+ tmp = 1.0f/A[k*n+k]; // invert pivot element
+ A[k*n+k] = 1.0f; // This element of input matrix becomes result matrix
+
+ // Perform row reduction (divide every element by pivot)
+ for (j = 0; j < n; j++)
+ {
+ A[k*n+j] = A[k*n+j]*tmp;
+ }
+
+ // Now eliminate all other entries in this column
+ for (i = 0; i < n; i++)
+ {
+ if (i != k)
+ {
+ tmp = A[i*n+k];
+ A[i*n+k] = 0.0f; // The other place where in matrix becomes result mat
+ for (j = 0; j < n; j++)
+ {
+ A[i*n+j] = A[i*n+j] - A[k*n+j]*tmp;
+ }
+ }
+ }
+ }
+
+ // Done, now need to undo pivot row swaps by doing column swaps in reverse order
+ for (k = n-1; k >= 0; k--)
+ {
+ if (pivrows[k] != k)
+ {
+ for (i = 0; i < n; i++)
+ {
+ tmp = A[i*n+k];
+ A[i*n+k] = A[i*n+pivrows[k]];
+ A[i*n+pivrows[k]] = tmp;
+ }
+ }
+ }
+ return 1;
+}
@@ -0,0 +1,27 @@
+/*
+ * MatrixMath.h Library for Matrix Math
+ *
+ * Created by Charlie Matlack on 12/18/10.
+ * Modified from code by RobH45345 on Arduino Forums, taken from unknown source.
+ */
+
+#ifndef MatrixMath_h
+#define MatrixMath_h
+
+#include "WProgram.h"
+
+class MatrixMath
+{
+public:
+ MatrixMath();
+ void MatrixPrint(float* A, int m, int n, String label);
+ void MatrixCopy(float* A, int n, int m, float* B);
+ void MatrixMult(float* A, float* B, int m, int p, int n, float* C);
+ void MatrixAdd(float* A, float* B, int m, int n, float* C);
+ void MatrixSubtract(float* A, float* B, int m, int n, float* C);
+ void MatrixTranspose(float* A, int m, int n, float* C);
+ int MatrixInvert(float* A, int n);
+ int LinearSolve(float* A, float* x, float* y, int n);
+};
+
+#endif
Oops, something went wrong.

0 comments on commit da4ec82

Please sign in to comment.