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

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