MatrixMath.cpp

Sat, 07 Nov 2015 13:24:46 +0100

author
mbayer
date
Sat, 07 Nov 2015 13:24:46 +0100
changeset 1
b584642d4f58
parent 0
2c8ba1964db7
permissions
-rw-r--r--

several modifications to support laser enable - still needs cleanup

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

mercurial