15 using namespace matrix;
19 bool comparetoidentity(
const Matrix& m,
double eps = EPS) {
21 D maxunitydeviation = 0.0;
22 D currentunitydeviation;
23 for (
unsigned int i = 0; i < m.
getM(); i++ ) {
24 currentunitydeviation = m.
val(i,i) - 1.;
25 if ( currentunitydeviation < 0) currentunitydeviation *= -1.;
26 if ( currentunitydeviation > maxunitydeviation ) {
27 maxunitydeviation = currentunitydeviation;
33 D maxzerodeviation = 0.0;
34 D currentzerodeviation ;
35 for (
unsigned int i = 0; i < m.
getM(); i++ ) {
36 for (
unsigned int j = 0; j < m.
getN(); j++ ) {
37 if ( i == j )
continue;
38 currentzerodeviation = m.
val(i,j);
39 if ( currentzerodeviation < 0) currentzerodeviation *= -1.0;
40 if ( currentzerodeviation > maxzerodeviation ) {
41 maxzerodeviation = currentzerodeviation;
53 return (maxunitydeviation < eps && maxzerodeviation < eps);
56 bool comparetozero(
const Matrix& m,
double eps = EPS) {
59 for (
unsigned int i = 0; i < m.
getM(); i++ ) {
60 for (
unsigned int j = 0; j < m.
getN(); j++ ) {
61 currentdeviation = m.
val(i,j);
62 if ( currentdeviation < 0.0) currentdeviation *= -1.;
63 if ( currentdeviation > maxdeviation ) {
64 maxdeviation = currentdeviation;
68 if(maxdeviation < eps)
71 cout <<
"\tWorst deviation: " << maxdeviation << endl;
80 cout <<
"\n -[ Creation and Access ]-\n";
82 D testdata[9]={1,0,0, 0,1,0, 0,0,1};
86 unit_assert(
"id=identity_set", M1 == M2 );
87 D testdata2[12]={1,2,3, 4,5,6, 7,8,9, 10,11,12};
89 unit_assert(
"dimension of matrix", M3.getM() == 4 && M3.getN() == 3 );
90 unit_assert(
"field check",
91 M3.val(0,2) == 3 && M3.val(2,1) == 8 && M3.val(3,0) == 10 );
93 unit_assert(
"copy constructor", M3 == M4 );
94 Matrix M5(1,3,testdata2+3);
95 unit_assert(
"row", M3.row(1) == M5 );
97 unit_assert(
"move constructor", M6 == M3 );
102 cout <<
"\n -[ Vector Operations ]-\n";
103 D testdata[3]={-1,3,2};
104 const Matrix V1(1,3, testdata);
105 const Matrix V2(3,1, testdata);
109 unit_assert(
"transpose", V3 == V2 );
111 unit_assert(
"double transpose", V3 == V1 );
112 D testdata2[3]={-2,6,4};
115 unit_assert(
"add", V3 == V4 );
116 D testdata3[3]={1,-3,-2};
119 unit_assert(
"sub", V4 == V5 );
121 D testdata4[3]={3,-9,-6};
125 unit_assert(
"mult with scalar I", V4 == V5 );
127 unit_assert(
"mult with scalar II", V4 == V5 );
134 unit_assert(
"scalarproduct", V4 == V6 );
136 unit_assert(
"scalarproduct with exp(2)", V4.multMT() == V6 );
138 unit_assert(
"vector^T*vector=matrix", V4.getM() == 3);
139 unit_assert(
"vector^T*vector=exp(2)", V3.multMT().getM() == 3);
145 cout <<
"\n -[ Matrix Operations ]-\n";
146 D testdata[6]={1,2,3, 4,5,6 };
147 const Matrix M1(2,3, testdata);
148 D testdata2[6]={1,4, 2,5, 3,6 };
149 const Matrix M2(3,2, testdata2);
153 unit_assert(
"transpose", M3 == M2 );
154 D testdata3[6]={2,4,6, 8,10,12};
157 unit_assert(
"add", M3 == M4 );
159 D testdata4[6]={-0.5, -1, -1.5, -2, -2.5, -3};
163 unit_assert(
"mult with scalar", M4 == M3 );
165 D testdata5[6] = {2, -1, 0, 3, 2, -2};
166 D testdata6[4] = {8, -1, 20, -1};
167 Matrix M5 (3,2, testdata5);
170 unit_assert(
"mult(matrix, matrix)", M4 == M6 );
175 unit_assert(
"exp(1)", M3 == M4 );
178 unit_assert(
"exp(T)=transpose", M3 == M4 );
181 unit_assert(
"exp(0)=id", M3 == M4 );
183 D testdata7[16] = {1,2,3,4, -4,2,1,3, 0.3,-0.9, 4, -3, 1,0.5,0.3,5.0};
188 unit_assert(
"exp(-1)*exp(1)=id", comparetoidentity(M4) );
189 M7=M8.pseudoInverse(0);
191 unit_assert(
"pseudoinverse*exp(1)=id", comparetoidentity(M4) );
193 D testdata9[6] = {sin(1.0),sin(2.0),sin(3.0), sin(4.0),sin(5.0),sin(6.0) };
197 unit_assert(
"map(sin)", M4 == M9 );
199 D testdata10[6] = {2,4,6, -0.4,-0.5,-0.6 };
200 D testdata11[2] = {2,-0.1};
201 Matrix M10(2,3,testdata10);
202 Matrix M11(2,1,testdata11);
204 M4.toMultrowwise(M11);
205 unit_assert(
"multrowwise()", M4 == M10 );
207 unit_assert(
"rowwise (&) ", M4 == M10 );
208 D testdata12[6] = {2,1,0, 8, 2.5, 0 };
209 D testdata13[3] = {2, 0.5, 0};
210 M10.set(2,3,testdata12);
211 Matrix M12(3,1,testdata13);
213 M4.toMultcolwise(M12);
214 unit_assert(
"multcolwise()", M4 == M10 );
221 unit_assert(
"multMT() ", M5 == M6 );
224 unit_assert(
"multTM() ", M5 == M6 );
226 D testdata20[12]={1,2,3, 4,5,6, 1,2,3, 4,5,6 };
227 const Matrix M20(4,3, testdata20);
229 unit_assert(
"above() ", M20 == M21 );
230 D testdata22[8]={1,2,3, 7, 4,5,6, 8};
231 D testdata23[2]={7, 8};
232 const Matrix M22(2,4, testdata22);
233 const Matrix M23(2,1, testdata23);
235 unit_assert(
"beside() ", M24 == M22 );
239 unit_assert(
"removeColumns() ", M31 == M1 );
242 unit_assert(
"removeRows() ", M33 == M1 );
248 cout <<
"\n -[ Matrix Operators (+ - * ^)]-\n";
249 D testdata[6]={1,2,3, 4,5,6 };
250 const Matrix M1(2,3, testdata);
251 D testdata2[6]={1,4, 2,5, 3,6 };
252 const Matrix M2(3,2, testdata2);
253 unit_assert(
"^T ", (M1^
T) == M2 );
254 D testdata3[6]={2,4,6, 8,10,12};
256 unit_assert(
"+ ", M1+M1 == M4 );
257 unit_assert(
"- ", M1+M1-M1 == M1 );
259 D testdata4[6]={-0.5, -1, -1.5, -2, -2.5, -3};
260 Matrix M3(2,3, testdata4);
261 unit_assert(
"* scalar", M1*(-0.5) == M3 );
263 D testdata5[6] = {2, -1, 0, 3, 2, -2};
264 D testdata6[4] = {8, -1, 20, -1};
265 Matrix M5 (3,2, testdata5);
267 unit_assert(
"* ", M1*M5 == M6 );
269 unit_assert(
"^1 ", (M1^1) == M1 );
271 unit_assert(
"^0=id ", (M1^0) == M3 );
273 D testdata7[16] = {1,2,3,4, -4,2,1,3, 0.3,-0.9, 4, -3, 1,0.5,0.3,5.0};
275 unit_assert(
"^1 * ^-1=id ", comparetoidentity(M7*(M7^-1)) );
280 cout <<
"\n -[ Matrix Utils: Eigenvalues and Eigenvectors ]-\n";
281 D testdata[9]={1,2,3, 4,5,6, 7,8,9};
282 const Matrix M1(3,3, testdata);
286 D resultval[3]={1.5*(95+sqrt(8881)), 1.5*(95-sqrt(8881)), 0};
287 const Matrix resultvalM(3,1, resultval);
289 unit_assert(
"Sym Real Eigenvalues ", comparetozero(eval-resultvalM));
291 D resultvecs[9]={0.21483723836839594,0.8872306883463704,0.4082482904638631,
292 0.5205873894647369,0.24964395298829792,-0.816496580927726,
293 0.826337540561078,-0.3879427823697746,0.4082482904638631};
294 const Matrix resultvecsM(3,3, resultvecs);
297 unit_assert(
"Sym Real Eigenvalues and Vectors: Vals", comparetozero(eval-resultvalM));
298 unit_assert(
"Sym Real Eigenvalues and Vectors: Vectors", comparetozero(evec-resultvecsM));
300 D testdata2[16]= {-1., 1., -1., 1., -8., 4., -2., 1., 27., 9., 3., 1., 64., 16., 4., 1.};
309 const Matrix M2(4,4, testdata2);
312 D resultval_r[4]={-6.413911026270929,5.5455534989094595, 5.5455534989094595, 2.3228040284520177};
313 D resultval_i[4]={0,3.0854497586289216,-3.0854497586289216,0};
314 const Matrix resultvalM_r(4,1, resultval_r);
315 const Matrix resultvalM_i(4,1, resultval_i);
317 unit_assert(
"NonSym Eigenvalues (Complex) ", comparetozero(eval_r-resultvalM_r)
318 && comparetozero(eval_i-resultvalM_i));
324 -0.09988217466836526,-0.043500372394264235,-0.043500372394264235,-0.14493294424802267,
325 -0.11125130967436689, 0.06398640593013502, 0.06398640593013502, 0.35660144308731107,
326 0.2925006732813017, -0.5151803514845115, -0.5151803514845115, 0.9193688436883699,
327 0.9445051897206503 ,-0.8405935042246232, -0.8405935042246232, 0.0811836295983173};
329 0, -0.007553928841000267, 0.007553928841000267,0,
330 0, -0.1422404507165189, 0.1422404507165189, 0,
331 0, 0.04142240814335418, -0.04142240814335418, 0,
333 Matrix resultvecsM_r(4,4, resultvecs_r);
334 Matrix resultvecsM_i(4,4, resultvecs_i);
336 unit_assert(
"NonSym Eigenvalues and Vectors: Vals",
337 comparetozero(eval_r-resultvalM_r) && comparetozero(eval_i-resultvalM_i) );
338 unit_assert(
"NonSym Eigenvalues and Vectors: Vectors",
339 comparetozero(evec_r-resultvecsM_r, 0.05) &&
340 comparetozero(evec_i-resultvecsM_i, 0.05));
346 cout <<
"\n -[ Speed: Inverion ]-\n";
348 cout <<
" DEBUG MODE! use -DNDEBUG -O3 (not -g) to get full performance\n";
352 D testdata0[9] = {1,2, -4,2};
354 UNIT_MEASURE_START(
"2x2 Matrix inversion", 100000)
356 UNIT_MEASURE_STOP("");
357 unit_assert( "validation", comparetoidentity(M1*M2));
360 D testdata1[9] = {1,2,3, -4,2,1, 0.3,-0.9};
362 UNIT_MEASURE_START(
"3x3 Matrix inversion", 100000)
364 UNIT_MEASURE_STOP("");
365 unit_assert( "validation", comparetoidentity(M1*M3));
367 D testdata2[16] = {1,2,3,4, -4,2,1,3, 0.3,-0.9, 4, -3, 1,0.5,0.3,5.0};
369 UNIT_MEASURE_START(
"4x4 Matrix inversion", 100000)
371 UNIT_MEASURE_STOP("");
372 unit_assert( "validation", comparetoidentity(M1*M4));
375 for (
unsigned int i=0; i < M20.getM(); i++)
376 for (
unsigned int j=0; j < M20.getN(); j++) {
377 M20.
val(i,j) = -22+(100. * rand())/RAND_MAX;
379 UNIT_MEASURE_START(
"20x20 Matrix inversion",1000)
381 UNIT_MEASURE_STOP("");
382 unit_assert( "validation", comparetoidentity(M1*M20));
386 for (
unsigned int i=0; i < M200.getM(); i++)
387 for (
unsigned int j=0; j < M200.getN(); j++) {
388 M200.
val(i,j) = -22+(100. * rand())/RAND_MAX;
390 UNIT_MEASURE_START(
"200x200 Matrix inversion",2)
392 UNIT_MEASURE_STOP("");
393 unit_assert( "validation", comparetoidentity(M1*M200));
395 cout << "\n -[ Speed: Other Operations ]-\n";
396 UNIT_MEASURE_START("20x20
Matrix multiplication with assignment",5000)
398 UNIT_MEASURE_STOP("");
399 UNIT_MEASURE_START("20x20
Matrix addition with assignment",100000)
401 UNIT_MEASURE_STOP("");
402 UNIT_MEASURE_START("20x20
Matrix inplace addition",100000)
404 UNIT_MEASURE_STOP("");
405 UNIT_MEASURE_START("20x20
Matrix transposition",100000)
407 UNIT_MEASURE_STOP("");
408 const
Matrix& M20Sym = M20.multMT();
409 UNIT_MEASURE_START("20x20
Matrix Sym Real Eigenvalues",1000)
412 UNIT_MEASURE_STOP("");
420 cout <<
"\n -[ Store and Restore ]-\n";
422 for(
int i =0; i<32; i++){
423 M1.
val(0,0) = (double)rand()/RAND_MAX;
426 for(
int i =0; i<64; i++){
427 M2.val(i%32,i/32) = (double)rand()/RAND_MAX;
430 f=fopen(
"test.dat",
"w");
434 f=fopen(
"test.dat",
"r");
439 unit_assert(
"validation", comparetozero(M1-M3,1e-6));
440 unit_assert(
"validation", comparetozero(M2-M4,1e-6));
447 double clip(
double r,
double x){
448 if(!isnormal(x))
return 0;
449 return x < -r ? -r : (x>r ? r : x);
454 cout <<
"\n -[ Inverion of Singular Matrices ]-\n";
457 D testdata0[9] = {1.0,0.0, 0.0,0.0};
460 cout << M1*M2 <<endl;
461 unit_assert(
"2x2 validation", 1 );
463 D testdata1[9] = {1,2,3, 1,2,3, 0.3,-0.9,.2};
466 unit_assert(
"3x3 validation", 1 );
467 cout << M3*M1 << endl;
469 D testdata2[16] = {1,2,3,4, 1,2,3,4, 0.3,-0.9, 4, -3, 1,0.5,0.3,5.0};
472 unit_assert(
"4x4 validation", 1 );
473 cout << M4*M1 << endl;
475 D testdata3[16] = {1,2,3,4, 1,0,3,4, 0.3,-0.9, 4, -3, 1,0.5,0.3,5.0};
478 unit_assert(
"4x4 validation", 1 );
479 cout << M1*M5 << endl;
484 UNIT_TEST_RUN(
"Matrix Tests" )
485 ADD_TEST( check_creation )
486 ADD_TEST( check_vector_operation )
487 ADD_TEST( check_matrix_operation )
488 ADD_TEST( check_matrix_operators )
489 ADD_TEST( check_matrix_utils )
491 ADD_TEST( store_restore )
492 ADD_TEST( invertzero )
Matrix type.
Definition: matrix.h:65
UNIT_TEST_DEFINES DEFINE_TEST(CheckSetGet)
Definition: configurabletest.cpp:51
I getM() const
Definition: matrix.h:88
Matrixd Matrix
Definition: osgforwarddecl.h:47
Matrix beside(const Matrix &a) const
returns a matrix that consists of this left beside A (number of columns is getN + a...
Definition: matrix.cpp:708
D val(I i, I j) const
Definition: matrix.h:96
Matrix eigenValuesRealSym(const Matrix &m)
calculates the eigenvalues of the real and symmetric matrix m and returns them as a column vector in ...
Definition: matrixutils.cpp:26
bool restore(FILE *f)
reads a Matrix from the given file stream uses read (or old binary format)
Definition: matrix.cpp:221
I getN() const
Definition: matrix.h:90
bool eigenValues(const Matrix &m, Matrix &real, Matrix &imag)
calculates the eigenvalues of the matrix m and returns them as a column vectors seperated to real and...
Definition: matrixutils.cpp:71
Matrix & removeRows(I numberRows)
removes one or more rows from the end if an existing matrix (inplace!), same as reshape(getM()-number...
Definition: matrix.cpp:736
Matrix above(const Matrix &a) const
returns a matrix that consists of this matrix above A (number of rows is getM + a.getM())
Definition: matrix.cpp:700
double clip(double r, double x)
clipping function for mapP
Definition: controller_misc.cpp:39
bool store(FILE *f) const
stores the Matrix into the given file stream (same as write)
Definition: matrix.cpp:208
Matrix & removeColumns(I numberColumns)
removes one or more columns from the end of the existing matrix (inplace!) resets the size of the mat...
Definition: matrix.cpp:741
Matrix multMT() const
optimised multiplication of Matrix with its transposed: M * M^T
Definition: matrix.cpp:637
const int T
integer constant for use with exp function and (^) operator to transpose the matrix ...
Definition: matrix.cpp:21
std::vector< int > toPositiveSignEigenVectors(Matrix &vecs_real, Matrix &vecs_imag)
flips the signs of the eigenvectors such that their first entry has positive real part...
Definition: matrixutils.cpp:225
Matrix secureInverse() const
calculates the secure inverse of a square matrix.
Definition: matrix.cpp:424
bool eigenValuesVectorsRealSym(const Matrix &m, Matrix &eigenvalues, Matrix &eigenvectors)
calculates the eigenvalues and corresponding eigenvectors of the the real and symmetric matrix m...
Definition: matrixutils.cpp:46
double D
type for matrix elements
Definition: matrix.h:38
bool eigenValuesVectors(const Matrix &m, Matrix &vals_real, Matrix &vals_imag, Matrix &vecs_real, Matrix &vecs_imag)
calculates the eigenvalues and corresponding eigenvectors of the matrix m.
Definition: matrixutils.cpp:92