36 #include <qpOASES.hpp> 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;
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 );
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);
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};
93 real_t Bv[] = {240, 16, -120, -140,
94 -2700, -120, 1200, 1680,
95 6480, 240, -2700, -4200,
96 -4200, -140, 1680, 2800};
104 for (i = 0; i < 16; i++) Av[i] = _Av[i];
106 A.
times(4, 1.0, Bv, 4, 0.0, Cv, 4);
109 for (j = 0; j < 4; j++)
111 for (i = 0; i < 4; i++)
113 d =
getAbs(Cv[j*4+i] - static_cast<real_t>(i == j));
114 if (d > err) err = d;
117 fprintf(
stdFile,
"Hilbert; Deviation from identity: %9.2e\n", err);
135 real_t _Asubv[] = {1.0/3.0, 0.25,
139 real_t Bsubv[] = {240, 16, -120, -140,
140 -2700, -120, 1200, 1680};
145 for (i = 0; i < 8; i++) Asubv[i] = _Asubv[i];
149 Asub.
transTimes(2, 1.0, Bsubv, 4, 0.0, Csubv, 2);
152 for (j = 0; j < 2; j++)
154 for (i = 0; i < 2; i++)
156 d =
getAbs(Csubv[j*2+i] - static_cast<real_t>(i == j));
157 if (d > err) err = d;
160 fprintf(
stdFile,
"Submatrix transpose; Deviation from identity: %9.2e\n", err);
174 const int_t M = 20,
N = 15, K = 5;
179 real_t *Av, *X, *Y, *x, *
y, *xc, *yc;
184 for (i = 0; i < m; i++) { rows.addNumber(M-1 - 2*i); rows.addNumber(2*i); }
189 rows.getNumberArray(&rNum);
191 for (i = 0; i < m; i++) fprintf(
stdFile,
" %2d", (
int)(rNum[i]) );
196 for (i = 0; i < n; i++) fprintf(
stdFile,
" %2d", (
int)(cNum[i]) );
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);
214 A.
times(&rows, &cols, K, 1.0, X, n, 0.0, y, m);
217 for (j = 0; j < m; j++)
219 for (k = 0; k < K; k++)
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];
227 fprintf(
stdFile,
"Indexlist submatrix; error: %9.2e\n", err);
230 A.
transTimes(&rows, &cols, K, 1.0, Y, m, 0.0, x, n);
234 for (j = 0; j < n; j++)
236 for (k = 0; k < K; k++)
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];
244 fprintf(
stdFile,
"Indexlist transpose submatrix; error: %9.2e\n", errT);
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;
305 for (j = 0; j < 3; j++)
307 fprintf(
stdFile,
"Column %ld:\n", j);
309 for (i = 0; i < 4; i++)
310 fprintf(
stdFile,
" %3.0f\n", col[i]);
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;
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;
360 for (j = 0; j < 3; j++)
363 for (i = 0; i < 4; i++)
364 fprintf(
stdFile,
" %3.0f", row[i]);
386 real_t Ax[] = {-23, -11, -26, 42, 74, 99};
387 real_t ATy[] = {-63, -69, -222, -423, -359, 272, 126, 663, 1562, 1656};
390 for (i = 0; i < 10; i++) x[i] = -4.0 + (
double)i;
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;
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;
409 A.
times(2, 1.0, x, 5, 0.0, y, 3);
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);
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);
438 long nRows = 2 * N + 1;
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};
461 for (i = 0; i < nCols; i++)
466 for (j = 0; j <
N; j++)
467 for (i = 0; i < 3; i++)
470 val[j*3+i] = 1.0 - 0.1 * (double)(j*3+i);
474 fprintf(
stdFile,
"Test matrix A =\n");
475 for (j = 0; j < nRows; j++)
478 for (i = 0; i < nCols; i++)
479 fprintf(
stdFile,
"%6.2f", xc[i]);
483 for (i = 0; i < 6; i++)
484 xc[i] = (1.0 + (
double)i) * 0.1;
486 A.
times(&rows, &cols, 2, 1.0, xc, 3, 0.0, yc, 4,
BT_TRUE);
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);
493 A.
transTimes(&rows, &cols, 2, 1.0, yc, 4, 0.0, xc, 3);
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);
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;
556 for (j = 0; j < 3; j++)
558 fprintf(
stdFile,
"Column %ld:\n", j);
560 for (i = 0; i < 4; i++)
561 fprintf(
stdFile,
" %3.0f\n", col[i]);
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;
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;
615 for (j = 0; j < 3; j++)
618 for (i = 0; i < 4; i++)
619 fprintf(
stdFile,
" %3.0f", row[i]);
641 real_t Ax[] = {-23, -11, -26, 42, 74, 99};
642 real_t ATy[] = {-63, -69, -222, -423, -359, 272, 126, 663, 1562, 1656};
645 for (i = 0; i < 10; i++) x[i] = -4.0 + (
double)i;
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;
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;
668 A.
times(2, 1.0, x, 5, 0.0, y, 3);
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);
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);
697 long nRows = 2 * N + 1;
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};
720 for (i = 0; i < nCols; i++)
725 for (j = 0; j <
N; j++)
726 for (i = 0; i < 3; i++)
729 val[j*3+i] = 1.0 - 0.1 * (double)(j*3+i);
737 fprintf(
stdFile,
"Test matrix A =\n");
738 for (j = 0; j < nRows; j++)
741 for (i = 0; i < nCols; i++)
742 fprintf(
stdFile,
"%6.2f", xc[i]);
746 for (i = 0; i < 6; i++)
747 xc[i] = (1.0 + (
double)i) * 0.1;
749 A.
times(&rows, &cols, 2, 1.0, xc, 3, 0.0, yc, 4,
BT_TRUE);
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);
756 A.
transTimes(&rows, &cols, 2, 1.0, yc, 4, 0.0, xc, 3);
757 for (i = 0; i < 3; i++)
759 for (j = 0; j < 2; j++)
760 fprintf(
stdFile,
"%6.2f", ATy[i + j*3]);
763 for (i = 0; i < 3; i++)
765 for (j = 0; j < 2; j++)
766 fprintf(
stdFile,
"%6.2f", xc[i + j*3]);
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);
793 real_t ZHZv[] = {0.144, 0.426, 0.708, 0.426, 1.500, 2.574, 0.708, 2.574, 4.440};
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);
807 for (i = 0; i < 6; ++i)
809 for (j = 0; j < 6; ++j)
810 fprintf (
stdFile,
"%3.3f ", Hv[i*6+j]);
819 for (i = 0; i < 18; i++) Z[i] = 0.1 * (i+1);
822 for (i = 0; i < 6; ++i)
824 for (j = 0; j < 3; ++j)
825 fprintf (
stdFile,
"%3.3f ", Z[i+j*6]);
830 Hd->
bilinear(cols, 3, Z, 6, ZHZd, 3);
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);
837 Hs->
bilinear(cols, 3, Z, 6, ZHZs, 3);
839 for (i = 0; i < 3; ++i)
841 for (j = 0; j < 3; ++j)
842 fprintf (
stdFile,
"%3.3f ", ZHZd[i*3+j]);
847 for (i = 0; i < 3; ++i)
849 for (j = 0; j < 3; ++j)
850 fprintf (
stdFile,
"%3.3f ", ZHZv[i*3+j]);
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);
#define USING_NAMESPACE_QPOASES
returnValue addNumber(int addnumber)
Interfaces matrix-vector operations tailored to symmetric sparse matrices.
virtual returnValue transTimes(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
returnValue getNumberArray(int *const numberarray) const
Interfaces matrix-vector operations tailored to symmetric dense matrices.
virtual returnValue getRow(int rNum, const Indexlist *const icols, real_t alpha, real_t *row) const
virtual returnValue getCol(int cNum, const Indexlist *const irows, real_t alpha, real_t *col) const
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
Interfaces matrix-vector operations tailored to general sparse matrices.
virtual returnValue times(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
virtual returnValue getRow(int_t rNum, const Indexlist *const icols, real_t alpha, real_t *row) const
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
Interfaces matrix-vector operations tailored to general dense matrices.
Interfaces matrix-vector operations tailored to general sparse matrices.
virtual returnValue getCol(int_t cNum, const Indexlist *const irows, real_t alpha, real_t *col) const
returnValue transTimes(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
USING_NAMESPACE_QPOASES int sumOfSquares()
#define QPOASES_TEST_FOR_TOL(x, tol)
virtual returnValue bilinear(const Indexlist *const icols, int xN, const real_t *x, int xLD, real_t *y, int yLD) const
Stores and manages index lists.
returnValue times(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
virtual returnValue bilinear(const Indexlist *const icols, int xN, const real_t *x, int xLD, real_t *y, int yLD) const
int indexDenseSubmatrix()