-
Notifications
You must be signed in to change notification settings - Fork 20
/
math-matrix.h
264 lines (245 loc) · 8.1 KB
/
math-matrix.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
/*!@addtogroup other
* @{
* @defgroup matrixfloat Matrix Library
* matrix Math Library
* @{
*/
#ifndef __MATH_MATRIX_FLOAT_H__
#define __MATH_MATRIX_FLOAT_H__
/** \file math-matrix.h
* \brief Matrix library
*
* math-matrix.h provides a number of frequently used functions that are useful for
* doing math with matrices.
* Taken from http://playground.arduino.cc/Code/MatrixMath and ported to ROBOTC.
* License: You may use this code as you wish, provided you give credit where its due.
*
* THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 4.10 AND HIGHER
*
* Changelog:
* - 0.1: Initial release
*
* \author Charlie Matlack
* \author RobH45345
* \author Xander Soldaat (xander_at_botbench.com)
* \date 2 March 2013
* \version 0.1
*/
#pragma systemFile
#define NR_END 1
#define MATRIX_MAX_SIZE 10
/**
* Prints a nicely formatted version of the matrix to the debugstream
*
* This function is for floats
* @param matrix the matrix to be printed
* @param numRows the number of rows in the matrix
* @param numCols the number of columns in the matrix
* @param label the label to use when printing the matrix
*/
void matrixPrintF(float* matrix, short numRows, short numCols, char* label){
// matrixA = input matrix (numRowsA x n)
short i,j;
writeDebugStreamLine(label);
for (i=0; i<numRows; i++){
for (j=0;j<numCols;j++){
writeDebugStream("%f", matrix[numCols*i+j]);
writeDebugStream("\t");
}
writeDebugStreamLine("");
}
}
/**
* Prints a nicely formatted version of the matrix to the debugstream
*
* This function is for longs
* @param matrix the matrix to be printed
* @param numRows the number of rows in the matrix
* @param numCols the number of columns in the matrix
* @param label the label to use when printing the matrix
*/
void matrixPrintL(long* matrix, short numRows, short numCols, char* label){
// matrixA = input matrix (numRowsA x n)
short i,j;
writeDebugStreamLine(label);
for (i=0; i<numRows; i++){
for (j=0;j<numCols;j++){
writeDebugStream("%d", matrix[numCols*i+j]);
writeDebugStream("\t");
}
writeDebugStreamLine("");
}
}
/**
* Copies all the values from one matrix into another
*
* This function is for floats
* @param source the matrix which is to be copied
* @param numRows the number of rows in the source matrix
* @param numCols the number of columns in the source matrix
* @param destination the matrix to copy to
*/
void matrixCopyF(float* source, short numRows, short numCols, float* destination)
{
memcpy(destination, source, numRows * numCols * sizeof(float));
}
/**
* Copies all the values from one matrix into another
*
* This function is for longs
* @param source the matrix which is to be copied
* @param numRows the number of rows in the source matrix
* @param numCols the number of columns in the source matrix
* @param destination the matrix to copy to
*/
void matrixCopyL(long* source, short numRows, short numCols, long* destination)
{
memcpy(destination, source, numRows * numCols * sizeof(long));
}
/**
* Copies all the values from one matrix into another
*
* This function is for floats
* @param matrixA the first matrix to be multiplied
* @param matrixB the second matrix to be multiplied
* @param numRowsA the number of rows in the first matrix
* @param numColsA the number of columns in the first matrix
* @param numColsB the number of columns in the second matrix
* @param matrixC the resulting matrix
*/
void matrixMultF(float* matrixA, float* matrixB, short numRowsA, short numColsA, short numColsB, float* matrixC)
{
short i, j, k;
for (i = 0; i < numRowsA; i++)
for(j = 0; j < numColsB; j++)
{
matrixC[numColsB * i + j] = 0;
for (k = 0; k < numColsA; k++)
matrixC[numColsB*i+j]= matrixC[numColsB*i+j]+matrixA[numColsA*i+k]*matrixB[numColsB*k+j];
}
}
/**
* Copies all the values from one matrix into another
*
* This function is for longs
* @param matrixA the first matrix to be multiplied
* @param matrixB the second matrix to be multiplied
* @param numRowsA the number of rows in the first matrix
* @param numColsA the number of columns in the first matrix
* @param numColsB the number of columns in the second matrix
* @param matrixC the resulting matrix
*/
void matrixMultL(long* matrixA, long* matrixB, short numRowsA, short numColsA, short numColsB, long* matrixC)
{
short i, j, k;
for (i = 0; i < numRowsA; i++)
for(j = 0; j < numColsB; j++)
{
matrixC[numColsB * i + j] = 0;
for (k = 0; k < numColsA; k++)
matrixC[numColsB*i+j]= matrixC[numColsB*i+j]+matrixA[numColsA*i+k]*matrixB[numColsB*k+j];
}
}
/**
* Adds values of two matrices
*
* This function is for floats
* @param matrixA the first matrix to be added
* @param matrixB the second matrix to be added
* @param numRowsA the number of rows of both matrices
* @param numColsA the number of columns of both matrices
* @param matrixC the resulting matrix
*/
void matrixAddF(float* matrixA, float* matrixB, short numRowsA, short numColsA, float* matrixC)
{
short i, j;
for (i = 0; i < numRowsA; i++)
for(j = 0; j < numColsA; j++)
matrixC[numColsA * i + j] = matrixA[numColsA * i + j] + matrixB[numColsA * i + j];
}
/**
* Adds values of two matrices
*
* This function is for longs
* @param matrixA the first matrix to be added
* @param matrixB the second matrix to be added
* @param numRowsA the number of rows of both matrices
* @param numColsA the number of columns of both matrices
* @param matrixC the resulting matrix
*/
void matrixAddL(long* matrixA, long* matrixB, short numRowsA, short numColsA, long* matrixC)
{
short i, j;
for (i = 0; i < numRowsA; i++)
for(j = 0; j < numColsA; j++)
matrixC[numColsA * i + j] = matrixA[numColsA * i + j] + matrixB[numColsA * i + j];
}
/**
* Adds values of two matrices
*
* This function is for floats
* @param matrixA the first matrix to be added
* @param matrixB the second matrix to be added
* @param numRowsA the number of rows of both matrices
* @param numColsA the number of columns of both matrices
* @param matrixC the resulting matrix
*/
void matrixSubtractF(float* matrixA, float* matrixB, short numRowsA, short numColsA, float* matrixC)
{
short i, j;
for (i = 0; i < numRowsA; i++)
for(j = 0; j < numColsA; j++)
matrixC[numColsA * i + j] = matrixA[numColsA * i +j ] - matrixB[numColsA * i + j];
}
/**
* Adds values of two matrices
*
* This function is for longs
* @param matrixA the first matrix to be added
* @param matrixB the second matrix to be added
* @param numRowsA the number of rows of both matrices
* @param numColsA the number of columns of both matrices
* @param matrixC the resulting matrix
*/
void matrixSubtractL(long* matrixA, long* matrixB, short numRowsA, short numColsA, long* matrixC)
{
short i, j;
for (i = 0; i < numRowsA; i++)
for(j = 0; j < numColsA; j++)
matrixC[numColsA * i + j] = matrixA[numColsA * i +j ] - matrixB[numColsA * i + j];
}
/**
* Transpose a matrix
*
* This function is for floats
* @param matrixA the first matrix to be transposed
* @param numRowsA the number of rows of matrixA
* @param numColsA the number of columns matrixA
* @param matrixC the resulting matrix
*/
void matrixTransposeF(float* matrixA, short numRowsA, short numColsA, float* matrixC)
{
short i, j;
for (i = 0;i < numRowsA; i++)
for(j = 0; j < numColsA; j++)
matrixC[numRowsA * j + i] = matrixA[numColsA * i + j];
}
/**
* Transpose a matrix
*
* This function is for longs
* @param matrixA the first matrix to be transposed
* @param numRowsA the number of rows of matrixA
* @param numColsA the number of columns matrixA
* @param matrixC the resulting matrix
*/
void matrixTransposeL(long* matrixA, short numRowsA, short numColsA, long* matrixC)
{
short i, j;
for (i = 0;i < numRowsA; i++)
for(j = 0; j < numColsA; j++)
matrixC[numRowsA * j + i] = matrixA[numColsA * i + j];
}
#endif // __MATH_MATRIX_FLOAT_H__
/* @} */
/* @} */