test_matrices.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of qpOASES.
3  *
4  * qpOASES -- An Implementation of the Online Active Set Strategy.
5  * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
6  * Christian Kirches et al. All rights reserved.
7  *
8  * qpOASES is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU Lesser General Public
10  * License as published by the Free Software Foundation; either
11  * version 2.1 of the License, or (at your option) any later version.
12  *
13  * qpOASES is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
16  * See the GNU Lesser General Public License for more details.
17  *
18  * You should have received a copy of the GNU Lesser General Public
19  * License along with qpOASES; if not, write to the Free Software
20  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
21  *
22  */
23 
24 
36 #include <qpOASES.hpp>
37 #include <qpOASES/UnitTesting.hpp>
38 
39 
40 USING_NAMESPACE_QPOASES
41 
42 
44 int sumOfSquares()
45 {
46  int_t i;
47 
48  /* sum of first n squares */
49  const int_t N = 100;
50  real_t *av = new real_t[N];
51  real_t *aTv = new real_t[N];
52  real_t *bv = new real_t[N];
53  real_t c;
54 
55  for (i = 0; i < N; i++) av[i] = i+1.0;
56  for (i = 0; i < N; i++) aTv[i] = i+1.0;
57  for (i = 0; i < N; i++) bv[i] = i+1.0;
58 
59  DenseMatrix a(1, N, N, av);
60  DenseMatrix aT(N, 1, 1, aTv);
61 
62  a.times(1, 1.0, bv, N, 0.0, &c, 1);
63  real_t err = c - (1.0/6.0)*N*(N+1)*(2*N+1);
64  fprintf(stdFile, "Dot product; Error in sum of first %d squares: %9.2e\n", (int)N, err );
65 
66  aT.transTimes(1, 1.0, bv, N, 0.0, &c, 1);
67  real_t errT = c - (1.0/6.0)*N*(N+1)*(2*N+1);
68  fprintf(stdFile, "Transpose; Error in sum of first %d squares: %9.2e\n", (int)N, errT);
69 
70  delete[] bv;
71  aT.free (); // or delete[] aTv;
72  a.free (); // or delete[] av;
73 
74  QPOASES_TEST_FOR_TOL( err ,1e-10 )
75  QPOASES_TEST_FOR_TOL( errT,1e-10 )
76 
77  return TEST_PASSED;
78 }
79 
80 
82 int hilbert()
83 {
84  int_t i, j;
85  real_t d, err;
86 
87  /* permuted 4x4 Hilbert matrix, row major format */
88  real_t _Av[] = {1.0/3.0, 1.0, 0.5, 0.25,
89  0.25, 0.5, 1.0/3.0, 0.2,
90  0.2, 1.0/3.0, 0.25, 1.0/6.0,
91  1.0/6.0, 0.25, 0.2, 1.0/7.0};
92  /* and its inverse, column major format */
93  real_t Bv[] = {240, 16, -120, -140,
94  -2700, -120, 1200, 1680,
95  6480, 240, -2700, -4200,
96  -4200, -140, 1680, 2800};
97 
98  /* result */
99  real_t *Av = new real_t[4*4];
100  real_t *Cv = new real_t[4*4];
101 
102  DenseMatrix A(4, 4, 4, Av);
103 
104  for (i = 0; i < 16; i++) Av[i] = _Av[i];
105 
106  A.times(4, 1.0, Bv, 4, 0.0, Cv, 4);
107 
108  err = 0.0;
109  for (j = 0; j < 4; j++)
110  {
111  for (i = 0; i < 4; i++)
112  {
113  d = getAbs(Cv[j*4+i] - static_cast<real_t>(i == j));
114  if (d > err) err = d;
115  }
116  }
117  fprintf(stdFile, "Hilbert; Deviation from identity: %9.2e\n", err);
118 
119  delete[] Cv;
120  A.free (); // or delete[] Av;
121 
122  QPOASES_TEST_FOR_TOL( err,1e-12 )
123 
124  return TEST_PASSED;
125 }
126 
127 
129 int submatrix()
130 {
131  int_t i, j;
132  real_t d, err;
133 
134  /* 2x3 transposed submatrix */
135  real_t _Asubv[] = {1.0/3.0, 0.25,
136  1.0, 0.5,
137  0.5, 1.0/3.0,
138  0.25, 0.2};
139  real_t Bsubv[] = {240, 16, -120, -140,
140  -2700, -120, 1200, 1680};
141  real_t Csubv[2*2];
142 
143  real_t *Asubv = new real_t[2*4];
144 
145  for (i = 0; i < 8; i++) Asubv[i] = _Asubv[i];
146 
147  DenseMatrix Asub(4, 2, 2, Asubv);
148 
149  Asub.transTimes(2, 1.0, Bsubv, 4, 0.0, Csubv, 2);
150 
151  err = 0.0;
152  for (j = 0; j < 2; j++)
153  {
154  for (i = 0; i < 2; i++)
155  {
156  d = getAbs(Csubv[j*2+i] - static_cast<real_t>(i == j));
157  if (d > err) err = d;
158  }
159  }
160  fprintf(stdFile, "Submatrix transpose; Deviation from identity: %9.2e\n", err);
161 
162  Asub.free (); // or delete[] Asubv;
163 
164  QPOASES_TEST_FOR_TOL( err,1e-13 )
165 
166  return TEST_PASSED;
167 }
168 
169 
171 int indexDenseSubmatrix()
172 {
173  /* dense submatrices via index lists */
174  const int_t M = 20, N = 15, K = 5;
175  int_t i, j, k, m, n;
176  Indexlist rows(N), cols(N);
177  int_t *rNum, *cNum;
178  real_t err=0.0, errT=0.0;
179  real_t *Av, *X, *Y, *x, *y, *xc, *yc;
180 
181  // prepare index lists
182  m = (M-3)/4;
183  n = (N-3)/4;
184  for (i = 0; i < m; i++) { rows.addNumber(M-1 - 2*i); rows.addNumber(2*i); }
185  for (i = 0; i < n; i++) { cols.addNumber(N-1 - 2*i); cols.addNumber(2*i); }
186  m *= 2;
187  n *= 2;
188 
189  rows.getNumberArray(&rNum);
190  fprintf(stdFile, "Rows: ");
191  for (i = 0; i < m; i++) fprintf(stdFile, " %2d", (int)(rNum[i]) );
192  fprintf(stdFile, "\n");
193 
194  cols.getNumberArray(&cNum);
195  fprintf(stdFile, "Cols: ");
196  for (i = 0; i < n; i++) fprintf(stdFile, " %2d", (int)(cNum[i]) );
197  fprintf(stdFile, "\n");
198 
199  // prepare input matrices
200  Av = new real_t[M*N];
201  X = new real_t[n*K];
202  Y = new real_t[m*K];
203  x = new real_t[n*K];
204  y = new real_t[m*K];
205  xc = new real_t[n*K];
206  yc = new real_t[m*K];
207 
208  DenseMatrix A(M, N, N, Av);
209  for (i = 0; i < M*N; i++) Av[i] = -0.5*N*M + (real_t)i;
210  for (i = 0; i < n*K; i++) X[i] = 1.0 / (real_t)(i+1);
211  for (i = 0; i < m*K; i++) Y[i] = 1.0 / (real_t)(i+1);
212 
213  // multiply
214  A.times(&rows, &cols, K, 1.0, X, n, 0.0, y, m);
215 
216  // check result
217  for (j = 0; j < m; j++)
218  {
219  for (k = 0; k < K; k++)
220  {
221  yc[j+k*m] = -y[j+k*m];
222  for (i = 0; i < n; i++)
223  yc[j+k*m] += Av[cNum[i]+rNum[j]*N] * X[i+k*n];
224  if (getAbs(yc[j+k*m]) > err) err = getAbs(yc[j+k*m]);
225  }
226  }
227  fprintf(stdFile, "Indexlist submatrix; error: %9.2e\n", err);
228 
229  // transpose multiply
230  A.transTimes(&rows, &cols, K, 1.0, Y, m, 0.0, x, n);
231 
232  // check result
233  errT = 0.0;
234  for (j = 0; j < n; j++)
235  {
236  for (k = 0; k < K; k++)
237  {
238  xc[j+k*n] = -x[j+k*n];
239  for (i = 0; i < m; i++)
240  xc[j+k*n] += Av[cNum[j]+rNum[i]*N] * Y[i+k*m];
241  if (getAbs(xc[j+k*n]) > errT) errT = getAbs(xc[j+k*n]);
242  }
243  }
244  fprintf(stdFile, "Indexlist transpose submatrix; error: %9.2e\n", errT);
245 
246  // check result
247 
248  // clean up
249  delete[] yc;
250  delete[] xc;
251  delete[] y;
252  delete[] x;
253  delete[] Y;
254  delete[] X;
255  A.free (); // or delete[] Av;
256 
257  QPOASES_TEST_FOR_TOL( err ,1e-13 )
258  QPOASES_TEST_FOR_TOL( errT,1e-14 )
259 
260  return TEST_PASSED;
261 }
262 
263 
265 int spGetCol()
266 {
267  long j, i;
268  sparse_int_t *ir = new sparse_int_t[10];
269  sparse_int_t *jc = new sparse_int_t[4];
270  real_t *val = new real_t[10];
271  real_t *col = new real_t[4];
272 
273  /* Test matrix:
274  *
275  * [ 0 3 6 ]
276  * [ 1 0 7 ]
277  * [ 2 0 8 ]
278  * [ 0 4 9 ]
279  * [ 0 5 10 ]
280  */
281 
282  jc[0] = 0; jc[1] = 2; jc[2] = 5; jc[3] = 10;
283  ir[0] = 1; ir[1] = 2;
284  ir[2] = 0; ir[3] = 3; ir[4] = 4;
285  ir[5] = 0; ir[6] = 1; ir[7] = 2; ir[8] = 3; ir[9] = 4;
286  for (i = 0; i < 10; i++) val[i] = 1.0 + (double)i;
287 
288  SparseMatrix A(5, 3, ir, jc, val);
289 
290  Indexlist rows(4);
291 
292  rows.addNumber(2);
293  rows.addNumber(4);
294  rows.addNumber(3);
295  rows.addNumber(0);
296 
297  /* Indexed matrix:
298  *
299  * [ 2 0 8 ]
300  * [ 0 5 10 ]
301  * [ 0 4 9 ]
302  * [ 0 3 6 ]
303  */
304 
305  for (j = 0; j < 3; j++)
306  {
307  fprintf(stdFile, "Column %ld:\n", j);
308  A.getCol( (int_t)j, &rows, 1.0, col );
309  for (i = 0; i < 4; i++)
310  fprintf(stdFile, " %3.0f\n", col[i]);
311  }
312 
313  delete[] col;
314  A.free (); // or delete[] val,jc,ir;
315 
316  return TEST_PASSED;
317 }
318 
319 
321 int spGetRow()
322 {
323  long j, i;
324  sparse_int_t *ir = new sparse_int_t[10];
325  sparse_int_t *jc = new sparse_int_t[6];
326  real_t *val = new real_t[10];
327  real_t *row = new real_t[4];
328 
329  /* Test matrix:
330  *
331  * [ 0 3 4 6 0 ]
332  * [ 1 0 0 7 9 ]
333  * [ 2 0 5 8 10 ]
334  */
335 
336  jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
337  ir[0] = 1; ir[1] = 2;
338  ir[2] = 0;
339  ir[3] = 0; ir[4] = 2;
340  ir[5] = 0; ir[6] = 1; ir[7] = 2;
341  ir[8] = 1; ir[9] = 2;
342  for (i = 0; i < 10; i++) val[i] = 1.0 + (double)i;
343 
344  SparseMatrix A(3, 4, ir, jc, val);
345 
346  Indexlist cols(4);
347 
348  cols.addNumber(2);
349  cols.addNumber(4);
350  cols.addNumber(3);
351  cols.addNumber(1);
352 
353  /* Indexed matrix:
354  *
355  * [ 4 0 6 3 ]
356  * [ 0 9 7 0 ]
357  * [ 5 10 8 0 ]
358  */
359 
360  for (j = 0; j < 3; j++)
361  {
362  A.getRow( (int_t)j, &cols, 1.0, row );
363  for (i = 0; i < 4; i++)
364  fprintf(stdFile, " %3.0f", row[i]);
365  fprintf(stdFile, "\n");
366  }
367 
368  delete[] row;
369  A.free ();
370 
371  return TEST_PASSED;
372 }
373 
374 
376 int spTimes()
377 {
378  long i;
379  sparse_int_t *ir = new sparse_int_t[10];
380  sparse_int_t *jc = new sparse_int_t[6];
381  real_t *val = new real_t[10];
382 
383  real_t *x = new real_t[5*2];
384  real_t *y = new real_t[3*2];
385 
386  real_t Ax[] = {-23, -11, -26, 42, 74, 99};
387  real_t ATy[] = {-63, -69, -222, -423, -359, 272, 126, 663, 1562, 1656};
388  real_t err=0.0, errT=0.0;
389 
390  for (i = 0; i < 10; i++) x[i] = -4.0 + (double)i;
391 
392  /* Test matrix:
393  *
394  * [ 0 3 4 6 0 ]
395  * [ 1 0 0 7 9 ]
396  * [ 2 0 5 8 10 ]
397  */
398 
399  jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
400  ir[0] = 1; ir[1] = 2;
401  ir[2] = 0;
402  ir[3] = 0; ir[4] = 2;
403  ir[5] = 0; ir[6] = 1; ir[7] = 2;
404  ir[8] = 1; ir[9] = 2;
405  for (i = 0; i < 10; i++) val[i] = 1.0 + (double)i;
406 
407  SparseMatrix A(3, 5, ir, jc, val); // reference to ir, jc, val
408 
409  A.times(2, 1.0, x, 5, 0.0, y, 3);
410 
411  for (i = 0; i < 6; i++)
412  if (getAbs(y[i] - Ax[i]) > err) err = getAbs(y[i] - Ax[i]);
413  fprintf(stdFile, "Error in sparse A*x: %9.2e\n", err);
414 
415  A.transTimes(2, 1.0, y, 3, 0.0, x, 5);
416 
417  errT = 0.0;
418  for (i = 0; i < 10; i++)
419  if (getAbs(x[i] - ATy[i]) > errT) errT = getAbs(x[i] - ATy[i]);
420  fprintf(stdFile, "Error in sparse A'*x: %9.2e\n", errT);
421 
422  A.free (); // or delete[] val,ir,jc
423  delete[] y;
424  delete[] x;
425 
426  QPOASES_TEST_FOR_TOL( err ,1e-15 )
427  QPOASES_TEST_FOR_TOL( errT,1e-15 )
428 
429  return TEST_PASSED;
430 }
431 
432 
434 int spIndTimes()
435 {
436  const long N = 4;
437  long i, j;
438  long nRows = 2 * N + 1;
439  long nCols = N;
440  long nnz = 3 * N;
441  sparse_int_t *ir = new sparse_int_t[nnz];
442  sparse_int_t *jc = new sparse_int_t[nCols+1];
443  real_t *val = new real_t[nnz];
444  real_t *xc = new real_t[3*2];
445  real_t *yc = new real_t[4*2];
446  real_t Ax[] = {0.31, 0.05, 0.06, 0.30, 0.76, 0.20, 0.24, 0.60};
447  real_t ATy[] = {0.278, 0.000, 0.548, 0.776, 0.000, 1.208};
448  real_t err=0.0, errT=0.0;
449 
450  Indexlist rows(4), cols(3), allcols( (int_t)nCols );
451 
452  rows.addNumber(2);
453  rows.addNumber(4);
454  rows.addNumber(3);
455  rows.addNumber(0);
456 
457  cols.addNumber(1);
458  cols.addNumber(3);
459  cols.addNumber(0);
460 
461  for (i = 0; i < nCols; i++)
462  allcols.addNumber( (int_t)i );
463 
464  // build test matrix
465  for (i = 0; i <= N; i++) jc[i] = (sparse_int_t)(3*i);
466  for (j = 0; j < N; j++)
467  for (i = 0; i < 3; i++)
468  {
469  ir[j*3+i] = (sparse_int_t)(2*j + i);
470  val[j*3+i] = 1.0 - 0.1 * (double)(j*3+i);
471  }
472  SparseMatrix A( (int_t)nRows, (int_t)nCols, ir, jc, val );
473 
474  fprintf(stdFile, "Test matrix A =\n");
475  for (j = 0; j < nRows; j++)
476  {
477  A.getRow( (int_t)j, &allcols, 1.0, xc );
478  for (i = 0; i < nCols; i++)
479  fprintf(stdFile, "%6.2f", xc[i]);
480  fprintf(stdFile, "\n");
481  }
482 
483  for (i = 0; i < 6; i++)
484  xc[i] = (1.0 + (double)i) * 0.1;
485 
486  A.times(&rows, &cols, 2, 1.0, xc, 3, 0.0, yc, 4, BT_TRUE);
487 
488  for (i = 0; i < 8; i++)
489  if (getAbs(yc[i] - Ax[i]) > err)
490  err = getAbs(yc[i] - Ax[i]);
491  fprintf(stdFile, "Error in sparse indexed A*x: %9.2e\n", err);
492 
493  A.transTimes(&rows, &cols, 2, 1.0, yc, 4, 0.0, xc, 3);
494  errT = 0.0;
495  for (i = 0; i < 6; i++)
496  if (getAbs(xc[i] - ATy[i]) > errT)
497  errT = getAbs(xc[i] - ATy[i]);
498  fprintf(stdFile, "Error in sparse indexed A'*y: %9.2e\n", errT);
499 
500  delete[] xc;
501  delete[] yc;
502  A.free ();
503 
504  QPOASES_TEST_FOR_TOL( err ,1e-15 )
505  QPOASES_TEST_FOR_TOL( errT,1e-15 )
506 
507  return TEST_PASSED;
508 }
509 
510 
512 int sprGetCol()
513 {
514  long j, i;
515  sparse_int_t *ir = new sparse_int_t[10];
516  sparse_int_t *jc = new sparse_int_t[4];
517  real_t *val = new real_t[10];
518  real_t *col = new real_t[4];
519 
520  /* Test matrix:
521  *
522  * [ 0 3 6 ]
523  * [ 1 0 7 ]
524  * [ 2 0 8 ]
525  * [ 0 4 9 ]
526  * [ 0 5 10 ]
527  */
528 
529  jc[0] = 0; jc[1] = 2; jc[2] = 5; jc[3] = 10;
530  ir[0] = 1; ir[1] = 2;
531  ir[2] = 0; ir[3] = 3; ir[4] = 4;
532  ir[5] = 0; ir[6] = 1; ir[7] = 2; ir[8] = 3; ir[9] = 4;
533  for (i = 0; i < 10; i++) val[i] = 1.0 + (double)i;
534 
535  SparseMatrix Ac(5, 3, ir, jc, val);
536  real_t *Acv = Ac.full(); // row major format
537  SparseMatrixRow A(5, 3, 3, Acv);
538  delete[] Acv;
539  Ac.free (); // or delete[] val,jc,ir;
540 
541  Indexlist rows(4);
542 
543  rows.addNumber(2);
544  rows.addNumber(4);
545  rows.addNumber(3);
546  rows.addNumber(0);
547 
548  /* Indexed matrix:
549  *
550  * [ 2 0 8 ]
551  * [ 0 5 10 ]
552  * [ 0 4 9 ]
553  * [ 0 3 6 ]
554  */
555 
556  for (j = 0; j < 3; j++)
557  {
558  fprintf(stdFile, "Column %ld:\n", j);
559  A.getCol( (int_t)j, &rows, 1.0, col );
560  for (i = 0; i < 4; i++)
561  fprintf(stdFile, " %3.0f\n", col[i]);
562  }
563 
564  delete[] col;
565  A.free (); // or delete[] val,jc,ir;
566 
567  return TEST_PASSED;
568 }
569 
570 
572 int sprGetRow()
573 {
574  long j, i;
575  sparse_int_t *ir = new sparse_int_t[10];
576  sparse_int_t *jc = new sparse_int_t[6];
577  real_t *val = new real_t[10];
578  real_t *row = new real_t[4];
579 
580  /* Test matrix:
581  *
582  * [ 0 3 4 6 0 ]
583  * [ 1 0 0 7 9 ]
584  * [ 2 0 5 8 10 ]
585  */
586 
587  jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
588  ir[0] = 1; ir[1] = 2;
589  ir[2] = 0;
590  ir[3] = 0; ir[4] = 2;
591  ir[5] = 0; ir[6] = 1; ir[7] = 2;
592  ir[8] = 1; ir[9] = 2;
593  for (i = 0; i < 10; i++) val[i] = 1.0 + (double)i;
594 
595  SparseMatrix Ac(3, 5, ir, jc, val);
596  real_t *Acv = Ac.full(); // row major format
597  SparseMatrixRow A(3, 5, 5, Acv);
598  delete[] Acv;
599  Ac.free (); // or delete[] val,jc,ir;
600 
601  Indexlist cols(4);
602 
603  cols.addNumber(2);
604  cols.addNumber(4);
605  cols.addNumber(3);
606  cols.addNumber(1);
607 
608  /* Indexed matrix:
609  *
610  * [ 4 0 6 3 ]
611  * [ 0 9 7 0 ]
612  * [ 5 10 8 0 ]
613  */
614 
615  for (j = 0; j < 3; j++)
616  {
617  A.getRow( (int_t)j, &cols, 1.0, row );
618  for (i = 0; i < 4; i++)
619  fprintf(stdFile, " %3.0f", row[i]);
620  fprintf(stdFile, "\n");
621  }
622 
623  delete[] row;
624  A.free ();
625 
626  return TEST_PASSED;
627 }
628 
629 
631 int sprTimes()
632 {
633  long i;
634  sparse_int_t *ir = new sparse_int_t[10];
635  sparse_int_t *jc = new sparse_int_t[6];
636  real_t *val = new real_t[10];
637 
638  real_t *x = new real_t[5*2];
639  real_t *y = new real_t[3*2];
640 
641  real_t Ax[] = {-23, -11, -26, 42, 74, 99};
642  real_t ATy[] = {-63, -69, -222, -423, -359, 272, 126, 663, 1562, 1656};
643  real_t err=0.0, errT=0.0;
644 
645  for (i = 0; i < 10; i++) x[i] = -4.0 + (double)i;
646 
647  /* Test matrix:
648  *
649  * [ 0 3 4 6 0 ]
650  * [ 1 0 0 7 9 ]
651  * [ 2 0 5 8 10 ]
652  */
653 
654  jc[0] = 0; jc[1] = 2; jc[2] = 3; jc[3] = 5; jc[4] = 8; jc[5] = 10;
655  ir[0] = 1; ir[1] = 2;
656  ir[2] = 0;
657  ir[3] = 0; ir[4] = 2;
658  ir[5] = 0; ir[6] = 1; ir[7] = 2;
659  ir[8] = 1; ir[9] = 2;
660  for (i = 0; i < 10; i++) val[i] = 1.0 + (double)i;
661 
662  SparseMatrix Ac(3, 5, ir, jc, val); // reference to ir, jc, val
663  real_t *Acv = Ac.full(); // row major format
664  SparseMatrixRow A(3, 5, 5, Acv);
665  delete[] Acv;
666  Ac.free (); // or delete[] val,jc,ir;
667 
668  A.times(2, 1.0, x, 5, 0.0, y, 3);
669 
670  for (i = 0; i < 6; i++)
671  if (getAbs(y[i] - Ax[i]) > err) err = getAbs(y[i] - Ax[i]);
672  fprintf(stdFile, "Error in sparse A*x: %9.2e\n", err);
673 
674  A.transTimes(2, 1.0, y, 3, 0.0, x, 5);
675 
676  errT = 0.0;
677  for (i = 0; i < 10; i++)
678  if (getAbs(x[i] - ATy[i]) > errT) errT = getAbs(x[i] - ATy[i]);
679  fprintf(stdFile, "Error in sparse A'*x: %9.2e\n", errT);
680 
681  A.free (); // or delete[] val,ir,jc
682  delete[] y;
683  delete[] x;
684 
685  QPOASES_TEST_FOR_TOL( err ,1e-15 )
686  QPOASES_TEST_FOR_TOL( errT,1e-15 )
687 
688  return TEST_PASSED;
689 }
690 
691 
693 int sprIndTimes()
694 {
695  const long N = 4;
696  long i, j;
697  long nRows = 2 * N + 1;
698  long nCols = N;
699  long nnz = 3 * N;
700  sparse_int_t *ir = new sparse_int_t[nnz];
701  sparse_int_t *jc = new sparse_int_t[nCols+1];
702  real_t *val = new real_t[nnz];
703  real_t *xc = new real_t[3*2];
704  real_t *yc = new real_t[4*2];
705  real_t Ax[] = {0.31, 0.05, 0.06, 0.30, 0.76, 0.20, 0.24, 0.60};
706  real_t ATy[] = {0.278, 0.000, 0.548, 0.776, 0.000, 1.208};
707  real_t err=0.0, errT=0.0;
708 
709  Indexlist rows(4), cols(3), allcols( (int_t)nCols );
710 
711  rows.addNumber(2);
712  rows.addNumber(4);
713  rows.addNumber(3);
714  rows.addNumber(0);
715 
716  cols.addNumber(1);
717  cols.addNumber(3);
718  cols.addNumber(0);
719 
720  for (i = 0; i < nCols; i++)
721  allcols.addNumber( (int_t)i );
722 
723  // build test matrix
724  for (i = 0; i <= N; i++) jc[i] = (sparse_int_t)(3*i);
725  for (j = 0; j < N; j++)
726  for (i = 0; i < 3; i++)
727  {
728  ir[j*3+i] = (sparse_int_t)(2*j + i);
729  val[j*3+i] = 1.0 - 0.1 * (double)(j*3+i);
730  }
731  SparseMatrix Ac( (int_t)nRows, (int_t)nCols, ir, jc, val);
732  real_t *Acv = Ac.full(); // row major format
733  SparseMatrixRow A( (int_t)nRows, (int_t)nCols, (int_t)nCols, Acv);
734  delete[] Acv;
735  Ac.free (); // or delete[] val,jc,ir;
736 
737  fprintf(stdFile, "Test matrix A =\n");
738  for (j = 0; j < nRows; j++)
739  {
740  A.getRow( (int_t)j, &allcols, 1.0, xc );
741  for (i = 0; i < nCols; i++)
742  fprintf(stdFile, "%6.2f", xc[i]);
743  fprintf(stdFile, "\n");
744  }
745 
746  for (i = 0; i < 6; i++)
747  xc[i] = (1.0 + (double)i) * 0.1;
748 
749  A.times(&rows, &cols, 2, 1.0, xc, 3, 0.0, yc, 4, BT_TRUE);
750 
751  for (i = 0; i < 8; i++)
752  if (getAbs(yc[i] - Ax[i]) > err)
753  err = getAbs(yc[i] - Ax[i]);
754  fprintf(stdFile, "Error in sparse indexed A*x: %9.2e\n", err);
755 
756  A.transTimes(&rows, &cols, 2, 1.0, yc, 4, 0.0, xc, 3);
757  for (i = 0; i < 3; i++)
758  {
759  for (j = 0; j < 2; j++)
760  fprintf(stdFile, "%6.2f", ATy[i + j*3]);
761  fprintf(stdFile, "\n");
762  }
763  for (i = 0; i < 3; i++)
764  {
765  for (j = 0; j < 2; j++)
766  fprintf(stdFile, "%6.2f", xc[i + j*3]);
767  fprintf(stdFile, "\n");
768  }
769  for (i = 0; i < 6; i++)
770  if (getAbs(xc[i] - ATy[i]) > errT)
771  errT = getAbs(xc[i] - ATy[i]);
772  fprintf(stdFile, "Error in sparse indexed A'*y: %9.2e\n", errT);
773 
774  delete[] xc;
775  delete[] yc;
776  A.free ();
777 
778  QPOASES_TEST_FOR_TOL( err ,1e-15 )
779  QPOASES_TEST_FOR_TOL( errT,1e-15 )
780 
781  return TEST_PASSED;
782 }
783 
784 
786 int symmetry()
787 {
788  int_t i,j;
789  real_t *Hv = new real_t[6*6];
790  real_t *Z = new real_t[6*3];
791  real_t *ZHZd = new real_t[3*3];
792  real_t *ZHZs = new real_t[3*3];
793  real_t ZHZv[] = {0.144, 0.426, 0.708, 0.426, 1.500, 2.574, 0.708, 2.574, 4.440};
794  real_t err=0.0, errS=0.0;
795  SymDenseMat *Hd;
796  SymSparseMat *Hs;
797  Indexlist *cols = new Indexlist(6);
798 
799  for (i = 0; i < 36; i++) Hv[i] = 0.0;
800  for (i = 0; i < 6; i++) Hv[i*7] = 1.0 - 0.1 * i;
801  for (i = 0; i < 5; i++) Hv[i*7+1] = Hv[i*7+6] = -0.1 * (i+1);
802 
803  Hd = new SymDenseMat(6, 6, 6, Hv); // deep-copy from Hv
804  Hs = new SymSparseMat(6, 6, 6, Hv); // deep-copy from Hv
805  Hs->createDiagInfo();
806 
807  for (i = 0; i < 6; ++i)
808  {
809  for (j = 0; j < 6; ++j)
810  fprintf (stdFile, "%3.3f ", Hv[i*6+j]);
811  fprintf (stdFile, "\n");
812  }
813  fprintf (stdFile, "\n");
814 
815  cols->addNumber(3);
816  cols->addNumber(0);
817  cols->addNumber(4);
818  cols->addNumber(1);
819  for (i = 0; i < 18; i++) Z[i] = 0.1 * (i+1);
820 
821  fprintf (stdFile, "\n");
822  for (i = 0; i < 6; ++i)
823  {
824  for (j = 0; j < 3; ++j)
825  fprintf (stdFile, "%3.3f ", Z[i+j*6]);
826  fprintf (stdFile, "\n");
827  }
828  fprintf (stdFile, "\n");
829 
830  Hd->bilinear(cols, 3, Z, 6, ZHZd, 3);
831 
832  for (i = 0; i < 9; i++)
833  if (getAbs(ZHZd[i] - ZHZv[i]) > err)
834  err = getAbs(ZHZd[i] - ZHZv[i]);
835  fprintf(stdFile, "Error in indexed dense bilinear form: %9.2e\n", err);
836 
837  Hs->bilinear(cols, 3, Z, 6, ZHZs, 3);
838 
839  for (i = 0; i < 3; ++i)
840  {
841  for (j = 0; j < 3; ++j)
842  fprintf (stdFile, "%3.3f ", ZHZd[i*3+j]);
843  fprintf (stdFile, "\n");
844  }
845  fprintf (stdFile, "\n");
846 
847  for (i = 0; i < 3; ++i)
848  {
849  for (j = 0; j < 3; ++j)
850  fprintf (stdFile, "%3.3f ", ZHZv[i*3+j]);
851  fprintf (stdFile, "\n");
852  }
853  fprintf (stdFile, "\n");
854 
855  for (i = 0; i < 9; i++)
856  if (getAbs(ZHZs[i] - ZHZv[i]) > errS)
857  errS = getAbs(ZHZs[i] - ZHZv[i]);
858  fprintf(stdFile, "Error in indexed sparse bilinear form: %9.2e\n", errS);
859 
860  delete cols;
861  delete Hs;
862  delete Hd;
863  delete[] ZHZs;
864  delete[] ZHZd;
865  delete[] Z;
866  delete[] Hv;
867 
868  QPOASES_TEST_FOR_TOL( err ,1e-15 )
869  QPOASES_TEST_FOR_TOL( errS,1e-15 )
870 
871  return TEST_PASSED;
872 }
873 
874 
876 int main()
877 {
878  int errorCount = TEST_PASSED;
879 
880  errorCount += sumOfSquares();
881  errorCount += hilbert();
882  errorCount += submatrix();
883  errorCount += indexDenseSubmatrix();
884 
885  errorCount += spGetCol();
886  errorCount += spGetRow();
887  errorCount += spTimes();
888  errorCount += spIndTimes();
889 
890  errorCount += sprGetCol();
891  errorCount += sprGetRow();
892  errorCount += sprTimes();
893  errorCount += sprIndTimes();
894 
895  errorCount += symmetry();
896 
897  return errorCount;
898 }
899 
900 
901 /*
902  * end of file
903  */
spGetRow
int spGetRow()
Definition: test_matrices.cpp:321
USING_NAMESPACE_QPOASES
#define USING_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:123
N
#define N
Definition: crane_kul_mhe_test.cpp:44
sprGetCol
int sprGetCol()
Definition: test_matrices.cpp:512
M
#define M
Definition: rocket_with_templates_ocp.hpp:43
SymSparseMat
Interfaces matrix-vector operations tailored to symmetric sparse matrices.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:586
SparseMatrix::transTimes
virtual returnValue transTimes(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:693
getAbs
real_t getAbs(real_t x)
spIndTimes
int spIndTimes()
Definition: test_matrices.cpp:434
SymDenseMat
Interfaces matrix-vector operations tailored to symmetric dense matrices.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:404
sprIndTimes
int sprIndTimes()
Definition: test_matrices.cpp:693
SparseMatrix::getRow
virtual returnValue getRow(int rNum, const Indexlist *const icols, real_t alpha, real_t *row) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:566
submatrix
int submatrix()
Definition: test_matrices.cpp:129
stdFile
#define stdFile
Definition: MessageHandling.h:56
sparse_int_t
int_t sparse_int_t
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:189
SparseMatrix::getCol
virtual returnValue getCol(int cNum, const Indexlist *const irows, real_t alpha, real_t *col) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:618
spGetCol
int spGetCol()
Definition: test_matrices.cpp:265
SparseMatrixRow::transTimes
virtual returnValue transTimes(int_t xN, real_t alpha, const real_t *x, int_t xLD, real_t beta, real_t *y, int_t yLD) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:1672
SparseMatrixRow
Interfaces matrix-vector operations tailored to general sparse matrices.
Definition: qpOASES-3.2.0/include/qpOASES/Matrices.hpp:782
int_t
int int_t
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:174
SparseMatrix::times
virtual returnValue times(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:656
SparseMatrixRow::getRow
virtual returnValue getRow(int_t rNum, const Indexlist *const icols, real_t alpha, real_t *row) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:1511
SparseMatrixRow::times
virtual returnValue times(int_t xN, real_t alpha, const real_t *x, int_t xLD, real_t beta, real_t *y, int_t yLD) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:1634
sprTimes
int sprTimes()
Definition: test_matrices.cpp:631
DenseMatrix
Interfaces matrix-vector operations tailored to general dense matrices.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:273
hilbert
int hilbert()
Definition: test_matrices.cpp:82
SparseMatrix
Interfaces matrix-vector operations tailored to general sparse matrices.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:443
SparseMatrixRow::getCol
virtual returnValue getCol(int_t cNum, const Indexlist *const irows, real_t alpha, real_t *col) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:1568
DenseMatrix::transTimes
returnValue transTimes(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:159
sumOfSquares
USING_NAMESPACE_QPOASES int sumOfSquares()
Definition: test_matrices.cpp:44
QPOASES_TEST_FOR_TOL
#define QPOASES_TEST_FOR_TOL(x, tol)
Definition: UnitTesting.hpp:61
SymSparseMat::bilinear
virtual returnValue bilinear(const Indexlist *const icols, int xN, const real_t *x, int xLD, real_t *y, int yLD) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:1108
TEST_PASSED
#define TEST_PASSED
Definition: UnitTesting.hpp:45
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
row
RowXpr row(Index i)
Definition: BlockMethods.h:725
DenseMatrix::times
returnValue times(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:151
SymDenseMat::bilinear
virtual returnValue bilinear(const Indexlist *const icols, int xN, const real_t *x, int xLD, real_t *y, int yLD) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:445
sprGetRow
int sprGetRow()
Definition: test_matrices.cpp:572
indexDenseSubmatrix
int indexDenseSubmatrix()
Definition: test_matrices.cpp:171
main
int main()
Definition: test_matrices.cpp:876
col
ColXpr col(Index i)
Definition: BlockMethods.h:708
real_t
double real_t
Definition: AD_test.c:10
symmetry
int symmetry()
Definition: test_matrices.cpp:786
spTimes
int spTimes()
Definition: test_matrices.cpp:376

acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:35:12