--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MatrixMath.cpp Sat Nov 07 13:23:07 2015 +0100
@@ -0,0 +1,206 @@
+/*
+ * 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 "Marlin.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_ECHOLN(' ');
+ SERIAL_ECHOLN(label);
+ for (i=0; i<m; i++){
+ for (j=0;j<n;j++){
+ serialPrintFloat(A[n*i+j]);
+ SERIAL_ECHO("\t");
+ }
+ SERIAL_ECHOLN(' ');
+ }
+}
+
+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_ECHOLNPGM("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;
+}
+
+void MatrixMath::MatrixIdentity(float* A, int m, int n)
+{
+ int i, j;
+ for (i=0;i<m;i++)
+ for(j=0;j<n;j++)
+ A[n*i+j]=i==j?1:0;
+}
+
+MatrixMath matrixMaths; //instance
\ No newline at end of file