Mercurial > hg-public > Marlin / file comparison

comparison: MatrixMath.cpp

MatrixMath.cpp

changeset 0
2c8ba1964db7
equal deleted inserted replaced
-1:000000000000 0:2c8ba1964db7
1 /*
2 * MatrixMath.cpp Library for MatrixMath
3 *
4 * Created by Charlie Matlack on 12/18/10.
5 * Modified from code by RobH45345 on Arduino Forums, taken from unknown source.
6 * MatrixMath.cpp
7 */
8
9 #include "Marlin.h"
10 #include "MatrixMath.h"
11
12 #define NR_END 1
13
14 MatrixMath::MatrixMath()
15 {
16 }
17
18 // Matrix Printing Routine
19 // Uses tabs to separate numbers under assumption printed float width won't cause problems
20 void MatrixMath::MatrixPrint(float* A, int m, int n, String label){
21 // A = input matrix (m x n)
22 int i,j;
23 SERIAL_ECHOLN(' ');
24 SERIAL_ECHOLN(label);
25 for (i=0; i<m; i++){
26 for (j=0;j<n;j++){
27 serialPrintFloat(A[n*i+j]);
28 SERIAL_ECHO("\t");
29 }
30 SERIAL_ECHOLN(' ');
31 }
32 }
33
34 void MatrixMath::MatrixCopy(float* A, int n, int m, float* B)
35 {
36 int i, j, k;
37 for (i=0;i<m;i++)
38 for(j=0;j<n;j++)
39 {
40 B[n*i+j] = A[n*i+j];
41 }
42 }
43
44 //Matrix Multiplication Routine
45 // C = A*B
46 void MatrixMath::MatrixMult(float* A, float* B, int m, int p, int n, float* C)
47 {
48 // A = input matrix (m x p)
49 // B = input matrix (p x n)
50 // m = number of rows in A
51 // p = number of columns in A = number of rows in B
52 // n = number of columns in B
53 // C = output matrix = A*B (m x n)
54 int i, j, k;
55 for (i=0;i<m;i++)
56 for(j=0;j<n;j++)
57 {
58 C[n*i+j]=0;
59 for (k=0;k<p;k++)
60 C[n*i+j]= C[n*i+j]+A[p*i+k]*B[n*k+j];
61 }
62 }
63
64
65 //Matrix Addition Routine
66 void MatrixMath::MatrixAdd(float* A, float* B, int m, int n, float* C)
67 {
68 // A = input matrix (m x n)
69 // B = input matrix (m x n)
70 // m = number of rows in A = number of rows in B
71 // n = number of columns in A = number of columns in B
72 // C = output matrix = A+B (m x n)
73 int i, j;
74 for (i=0;i<m;i++)
75 for(j=0;j<n;j++)
76 C[n*i+j]=A[n*i+j]+B[n*i+j];
77 }
78
79
80 //Matrix Subtraction Routine
81 void MatrixMath::MatrixSubtract(float* A, float* B, int m, int n, float* C)
82 {
83 // A = input matrix (m x n)
84 // B = input matrix (m x n)
85 // m = number of rows in A = number of rows in B
86 // n = number of columns in A = number of columns in B
87 // C = output matrix = A-B (m x n)
88 int i, j;
89 for (i=0;i<m;i++)
90 for(j=0;j<n;j++)
91 C[n*i+j]=A[n*i+j]-B[n*i+j];
92 }
93
94
95 //Matrix Transpose Routine
96 void MatrixMath::MatrixTranspose(float* A, int m, int n, float* C)
97 {
98 // A = input matrix (m x n)
99 // m = number of rows in A
100 // n = number of columns in A
101 // C = output matrix = the transpose of A (n x m)
102 int i, j;
103 for (i=0;i<m;i++)
104 for(j=0;j<n;j++)
105 C[m*j+i]=A[n*i+j];
106 }
107
108
109 //Matrix Inversion Routine
110 // * This function inverts a matrix based on the Gauss Jordan method.
111 // * Specifically, it uses partial pivoting to improve numeric stability.
112 // * The algorithm is drawn from those presented in
113 // NUMERICAL RECIPES: The Art of Scientific Computing.
114 // * The function returns 1 on success, 0 on failure.
115 // * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED
116 int MatrixMath::MatrixInvert(float* A, int n)
117 {
118 // A = input matrix AND result matrix
119 // n = number of rows = number of columns in A (n x n)
120 int pivrow; // keeps track of current pivot row
121 int k,i,j; // k: overall index along diagonal; i: row index; j: col index
122 int pivrows[n]; // keeps track of rows swaps to undo at end
123 float tmp; // used for finding max value and making column swaps
124
125 for (k = 0; k < n; k++)
126 {
127 // find pivot row, the row with biggest entry in current column
128 tmp = 0;
129 for (i = k; i < n; i++)
130 {
131 if (abs(A[i*n+k]) >= tmp) // 'Avoid using other functions inside abs()?'
132 {
133 tmp = abs(A[i*n+k]);
134 pivrow = i;
135 }
136 }
137
138 // check for singular matrix
139 if (A[pivrow*n+k] == 0.0f)
140 {
141 SERIAL_ECHOLNPGM("Inversion failed due to singular matrix");
142 return 0;
143 }
144
145 // Execute pivot (row swap) if needed
146 if (pivrow != k)
147 {
148 // swap row k with pivrow
149 for (j = 0; j < n; j++)
150 {
151 tmp = A[k*n+j];
152 A[k*n+j] = A[pivrow*n+j];
153 A[pivrow*n+j] = tmp;
154 }
155 }
156 pivrows[k] = pivrow; // record row swap (even if no swap happened)
157
158 tmp = 1.0f/A[k*n+k]; // invert pivot element
159 A[k*n+k] = 1.0f; // This element of input matrix becomes result matrix
160
161 // Perform row reduction (divide every element by pivot)
162 for (j = 0; j < n; j++)
163 {
164 A[k*n+j] = A[k*n+j]*tmp;
165 }
166
167 // Now eliminate all other entries in this column
168 for (i = 0; i < n; i++)
169 {
170 if (i != k)
171 {
172 tmp = A[i*n+k];
173 A[i*n+k] = 0.0f; // The other place where in matrix becomes result mat
174 for (j = 0; j < n; j++)
175 {
176 A[i*n+j] = A[i*n+j] - A[k*n+j]*tmp;
177 }
178 }
179 }
180 }
181
182 // Done, now need to undo pivot row swaps by doing column swaps in reverse order
183 for (k = n-1; k >= 0; k--)
184 {
185 if (pivrows[k] != k)
186 {
187 for (i = 0; i < n; i++)
188 {
189 tmp = A[i*n+k];
190 A[i*n+k] = A[i*n+pivrows[k]];
191 A[i*n+pivrows[k]] = tmp;
192 }
193 }
194 }
195 return 1;
196 }
197
198 void MatrixMath::MatrixIdentity(float* A, int m, int n)
199 {
200 int i, j;
201 for (i=0;i<m;i++)
202 for(j=0;j<n;j++)
203 A[n*i+j]=i==j?1:0;
204 }
205
206 MatrixMath matrixMaths; //instance

Mercurial Repository: Marlin

mercurial