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 
36 #include <qpOASES_e/Matrices.h>
37 
38 
39 BEGIN_NAMESPACE_QPOASES
40 
41 
42 /*****************************************************************************
43  * P U B L I C *
44  *****************************************************************************/
45 
46 
47 void DenseMatrixCON( DenseMatrix* _THIS,
48  int m,
49  int n,
50  int lD,
51  real_t *v
52  )
53 {
54  DenseMatrix_init( _THIS,m,n,lD,v );
55 }
56 
57 
58 void DenseMatrixCPY( DenseMatrix* FROM,
59  DenseMatrix* TO
60  )
61 {
62  int i,j;
63 
64  TO->nRows = FROM->nRows;
65  TO->nCols = FROM->nCols;
66  TO->leaDim = FROM->leaDim;
67 
68  for( i=0; i<FROM->nRows; ++i )
69  for( j=0; j<FROM->nCols; ++j )
70  TO->val[i*FROM->leaDim+j] = FROM->val[i*FROM->leaDim+j];
71 }
72 
73 
74 
75 void DenseMatrix_free( DenseMatrix* _THIS )
76 {
77 }
78 
79 
80 returnValue DenseMatrix_init( DenseMatrix* _THIS,
81  int m,
82  int n,
83  int lD,
84  real_t *v
85  )
86 {
87  int i,j;
88 
89  if ( n*m > NVCMAX*NVMAX )
90  return RET_INVALID_ARGUMENTS;
91 
92  _THIS->nRows = m;
93  _THIS->nCols = n;
94  _THIS->leaDim = lD;
95 
96  for( i=0; i<m; ++i )
97  for( j=0; j<n; ++j )
98  _THIS->val[i*lD+j] = v[i*n+j];
99 
100  /*qpOASES_printM( v,m,n );
101  qpOASES_printM( _THIS->val,m,n );*/
102 
103  return SUCCESSFUL_RETURN;
104 }
105 
106 
107 
108 real_t DenseMatrix_diag( DenseMatrix* _THIS,
109  int i
110  )
111 {
112  if ( ( i < 0 ) || ( i >= _THIS->nRows ) || ( i >= _THIS->nCols ) )
113  return -QPOASES_INFTY;
114  else
115  return _THIS->val[i*((_THIS->leaDim)+1)];
116 }
117 
118 
119 BooleanType DenseMatrix_isDiag( DenseMatrix* _THIS )
120 {
121  int i, j;
122 
123  if (_THIS->nRows != _THIS->nCols)
124  return BT_FALSE;
125 
126  for ( i=0; i<_THIS->nRows; ++i )
127  for ( j=0; j<i; ++j )
128  if ( ( fabs( _THIS->val[i*(_THIS->leaDim)+j] ) > QPOASES_EPS ) || ( fabs( _THIS->val[j*(_THIS->leaDim)+i] ) > QPOASES_EPS ) )
129  return BT_FALSE;
130 
131  return BT_TRUE;
132 }
133 
134 
135 real_t DenseMatrix_getNorm( DenseMatrix* _THIS, int type )
136 {
137  return REFER_NAMESPACE_QPOASES qpOASES_getNorm( _THIS->val,(_THIS->nCols)*(_THIS->nRows),type );
138 }
139 
140 
141 real_t DenseMatrix_getRowNorm( DenseMatrix* _THIS, int rNum, int type )
142 {
143  return REFER_NAMESPACE_QPOASES qpOASES_getNorm( &(_THIS->val[rNum*(_THIS->leaDim)]),_THIS->nCols,type );
144 }
145 
146 
147 returnValue DenseMatrix_getRow( DenseMatrix* _THIS,int rNum, const Indexlist* const icols, real_t alpha, real_t *row)
148 {
149  int i;
150  if (icols != 0)
151  {
152  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
153  for (i = 0; i < icols->length; i++)
154  row[i] = _THIS->val[rNum*(_THIS->leaDim)+icols->number[i]];
155  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
156  for (i = 0; i < icols->length; i++)
157  row[i] = -_THIS->val[rNum*(_THIS->leaDim)+icols->number[i]];
158  else
159  for (i = 0; i < icols->length; i++)
160  row[i] = alpha*_THIS->val[rNum*(_THIS->leaDim)+icols->number[i]];
161  }
162  else
163  {
164  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
165  for (i = 0; i < _THIS->nCols; i++)
166  row[i] = _THIS->val[rNum*(_THIS->leaDim)+i];
167  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
168  for (i = 0; i < _THIS->nCols; i++)
169  row[i] = -_THIS->val[rNum*(_THIS->leaDim)+i];
170  else
171  for (i = 0; i < _THIS->nCols; i++)
172  row[i] = alpha*_THIS->val[rNum*(_THIS->leaDim)+i];
173  }
174  return SUCCESSFUL_RETURN;
175 }
176 
177 
178 returnValue DenseMatrix_getCol( DenseMatrix* _THIS,int cNum, const Indexlist* const irows, real_t alpha, real_t *col)
179 {
180  int i;
181  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
182  for (i = 0; i < irows->length; i++)
183  col[i] = _THIS->val[irows->number[i]*(_THIS->leaDim)+cNum];
184  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
185  for (i = 0; i < irows->length; i++)
186  col[i] = -_THIS->val[irows->number[i]*(_THIS->leaDim)+cNum];
187  else
188  for (i = 0; i < irows->length; i++)
189  col[i] = alpha*_THIS->val[irows->number[i]*(_THIS->leaDim)+cNum];
190 
191  return SUCCESSFUL_RETURN;
192 }
193 
194 
195 
196 returnValue DenseMatrix_times( DenseMatrix* _THIS,
197  int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD
198  )
199 {
200  unsigned long _xN = (unsigned long)xN;
201  unsigned long _nRows = (unsigned long)(_THIS->nRows);
202  unsigned long _nCols = (unsigned long)(_THIS->nCols);
203  unsigned long _leaDim = (unsigned long)qpOASES_getMax(1,_THIS->nCols);
204  unsigned long _xLD = (unsigned long)qpOASES_getMax(1,xLD);
205  unsigned long _yLD = (unsigned long)qpOASES_getMax(1,yLD);
206 
207  /* Call BLAS. Mind row major format! */
208  GEMM("TRANS", "NOTRANS", &_nRows, &_xN, &_nCols, &alpha, _THIS->val, &_leaDim, x, &_xLD, &beta, y, &_yLD);
209  return SUCCESSFUL_RETURN;
210 }
211 
212 
213 returnValue DenseMatrix_transTimes( DenseMatrix* _THIS,
214  int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD
215  )
216 {
217  unsigned long _xN = (unsigned long)xN;
218  unsigned long _nRows = (unsigned long)(_THIS->nRows);
219  unsigned long _nCols = (unsigned long)(_THIS->nCols);
220  unsigned long _leaDim = (unsigned long)qpOASES_getMax(1,_THIS->nCols);
221  unsigned long _xLD = (unsigned long)qpOASES_getMax(1,xLD);
222  unsigned long _yLD = (unsigned long)qpOASES_getMax(1,yLD);
223 
224  /* Call BLAS. Mind row major format! */
225  GEMM("NOTRANS", "NOTRANS", &_nCols, &_xN, &_nRows, &alpha, _THIS->val, &_leaDim, x, &_xLD, &beta, y, &_yLD);
226  return SUCCESSFUL_RETURN;
227 }
228 
229 
230 returnValue DenseMatrix_subTimes( DenseMatrix* _THIS,
231  const Indexlist* const irows, const Indexlist* const icols,
232  int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD,
233  BooleanType yCompr
234  )
235 {
236  int i, j, k, row, col, iy, irA;
237 
238  if (yCompr == BT_TRUE)
239  {
240  if ( REFER_NAMESPACE_QPOASES qpOASES_isZero(beta,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
241  for (k = 0; k < xN; k++)
242  for (j = 0; j < irows->length; j++)
243  y[j+k*yLD] = 0.0;
244  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(beta,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
245  for (k = 0; k < xN; k++)
246  for (j = 0; j < irows->length; j++)
247  y[j+k*yLD] = -y[j+k*yLD];
248  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(beta,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_FALSE )
249  for (k = 0; k < xN; k++)
250  for (j = 0; j < irows->length; j++)
251  y[j+k*yLD] *= beta;
252 
253  if (icols == 0)
254  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
255  for (k = 0; k < xN; k++)
256  for (j = 0; j < irows->length; j++)
257  {
258  row = irows->iSort[j];
259  iy = row + k * yLD;
260  irA = irows->number[row] * (_THIS->leaDim);
261  for (i = 0; i < _THIS->nCols; i++)
262  y[iy] += _THIS->val[irA+i] * x[k*xLD+i];
263  }
264  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
265  for (k = 0; k < xN; k++)
266  for (j = 0; j < irows->length; j++)
267  {
268  row = irows->iSort[j];
269  iy = row + k * yLD;
270  irA = irows->number[row] * (_THIS->leaDim);
271  for (i = 0; i < _THIS->nCols; i++)
272  y[iy] -= _THIS->val[irA+i] * x[k*xLD+i];
273  }
274  else
275  for (k = 0; k < xN; k++)
276  for (j = 0; j < irows->length; j++)
277  {
278  row = irows->iSort[j];
279  iy = row + k * yLD;
280  irA = irows->number[row] * (_THIS->leaDim);
281  for (i = 0; i < _THIS->nCols; i++)
282  y[iy] += alpha * _THIS->val[irA+i] * x[k*xLD+i];
283  }
284  else /* icols != 0 */
285  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
286  for (k = 0; k < xN; k++)
287  for (j = 0; j < irows->length; j++)
288  {
289  row = irows->iSort[j];
290  iy = row + k * yLD;
291  irA = irows->number[row] * (_THIS->leaDim);
292  for (i = 0; i < icols->length; i++)
293  {
294  col = icols->iSort[i];
295  y[iy] += _THIS->val[irA+icols->number[col]] * x[k*xLD+col];
296  }
297  }
298  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
299  for (k = 0; k < xN; k++)
300  for (j = 0; j < irows->length; j++)
301  {
302  row = irows->iSort[j];
303  iy = row + k * yLD;
304  irA = irows->number[row] * (_THIS->leaDim);
305  for (i = 0; i < icols->length; i++)
306  {
307  col = icols->iSort[i];
308  y[iy] -= _THIS->val[irA+icols->number[col]] * x[k*xLD+col];
309  }
310  }
311  else
312  for (k = 0; k < xN; k++)
313  for (j = 0; j < irows->length; j++)
314  {
315  row = irows->iSort[j];
316  iy = row + k * yLD;
317  irA = irows->number[row] * (_THIS->leaDim);
318  for (i = 0; i < icols->length; i++)
319  {
320  col = icols->iSort[i];
321  y[iy] += alpha * _THIS->val[irA+icols->number[col]] * x[k*xLD+col];
322  }
323  }
324  }
325  else /* y not compressed */
326  {
327  if ( REFER_NAMESPACE_QPOASES qpOASES_isZero(beta,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
328  for (k = 0; k < xN; k++)
329  for (j = 0; j < irows->length; j++)
330  y[irows->number[j]+k*yLD] = 0.0;
331  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(beta,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
332  for (k = 0; k < xN; k++)
333  for (j = 0; j < irows->length; j++)
334  y[irows->number[j]+k*yLD] = -y[j+k*yLD];
335  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(beta,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_FALSE )
336  for (k = 0; k < xN; k++)
337  for (j = 0; j < irows->length; j++)
338  y[irows->number[j]+k*yLD] *= beta;
339 
340  if (icols == 0)
341  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
342  for (k = 0; k < xN; k++)
343  for (j = 0; j < irows->length; j++)
344  {
345  row = irows->number[irows->iSort[j]];
346  iy = row + k * yLD;
347  irA = row * (_THIS->leaDim);
348  for (i = 0; i < _THIS->nCols; i++)
349  y[iy] += _THIS->val[irA+i] * x[k*xLD+i];
350  }
351  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
352  for (k = 0; k < xN; k++)
353  for (j = 0; j < irows->length; j++)
354  {
355  row = irows->number[irows->iSort[j]];
356  iy = row + k * yLD;
357  irA = row * (_THIS->leaDim);
358  for (i = 0; i < _THIS->nCols; i++)
359  y[iy] -= _THIS->val[irA+i] * x[k*xLD+i];
360  }
361  else
362  for (k = 0; k < xN; k++)
363  for (j = 0; j < irows->length; j++)
364  {
365  row = irows->number[irows->iSort[j]];
366  iy = row + k * yLD;
367  irA = row * (_THIS->leaDim);
368  for (i = 0; i < _THIS->nCols; i++)
369  y[iy] += alpha * _THIS->val[irA+i] * x[k*xLD+i];
370  }
371  else /* icols != 0 */
372  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
373  for (k = 0; k < xN; k++)
374  for (j = 0; j < irows->length; j++)
375  {
376  row = irows->number[irows->iSort[j]];
377  iy = row + k * yLD;
378  irA = row * (_THIS->leaDim);
379  for (i = 0; i < icols->length; i++)
380  {
381  col = icols->iSort[i];
382  y[iy] += _THIS->val[irA+icols->number[col]] * x[k*xLD+col];
383  }
384  }
385  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
386  for (k = 0; k < xN; k++)
387  for (j = 0; j < irows->length; j++)
388  {
389  row = irows->number[irows->iSort[j]];
390  iy = row + k * yLD;
391  irA = row * (_THIS->leaDim);
392  for (i = 0; i < icols->length; i++)
393  {
394  col = icols->iSort[i];
395  y[iy] -= _THIS->val[irA+icols->number[col]] * x[k*xLD+col];
396  }
397  }
398  else
399  for (k = 0; k < xN; k++)
400  for (j = 0; j < irows->length; j++)
401  {
402  row = irows->number[irows->iSort[j]];
403  iy = row + k * yLD;
404  irA = row * (_THIS->leaDim);
405  for (i = 0; i < icols->length; i++)
406  {
407  col = icols->iSort[i];
408  y[iy] += alpha * _THIS->val[irA+icols->number[col]] * x[k*xLD+col];
409  }
410  }
411  }
412 
413  return SUCCESSFUL_RETURN;
414 }
415 
416 returnValue DenseMatrix_subTransTimes( DenseMatrix* _THIS,
417  const Indexlist* const irows, const Indexlist* const icols,
418  int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD
419  )
420 {
421  int i, j, k, row, col;
422 
423  if ( REFER_NAMESPACE_QPOASES qpOASES_isZero(beta,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
424  for (k = 0; k < xN; k++)
425  for (j = 0; j < icols->length; j++)
426  y[j+k*yLD] = 0.0;
427  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(beta,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
428  for (k = 0; k < xN; k++)
429  for (j = 0; j < icols->length; j++)
430  y[j+k*yLD] = -y[j+k*yLD];
431  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(beta,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_FALSE )
432  for (k = 0; k < xN; k++)
433  for (j = 0; j < icols->length; j++)
434  y[j+k*yLD] *= beta;
435 
436  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
437  for (k = 0; k < xN; k++)
438  for (j = 0; j < irows->length; j++)
439  {
440  row = irows->iSort[j];
441  for (i = 0; i < icols->length; i++)
442  {
443  col = icols->iSort[i];
444  y[col+k*yLD] += _THIS->val[irows->number[row]*(_THIS->leaDim)+icols->number[col]] * x[row+k*xLD];
445  }
446  }
447  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(alpha,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
448  for (k = 0; k < xN; k++)
449  for (j = 0; j < irows->length; j++)
450  {
451  row = irows->iSort[j];
452  for (i = 0; i < icols->length; i++)
453  {
454  col = icols->iSort[i];
455  y[col+k*yLD] -= _THIS->val[irows->number[row]*(_THIS->leaDim)+icols->number[col]] * x[row+k*xLD];
456  }
457  }
458  else
459  for (k = 0; k < xN; k++)
460  for (j = 0; j < irows->length; j++)
461  {
462  row = irows->iSort[j];
463  for (i = 0; i < icols->length; i++)
464  {
465  col = icols->iSort[i];
466  y[col+k*yLD] += alpha * _THIS->val[irows->number[row]*(_THIS->leaDim)+icols->number[col]] * x[row+k*xLD];
467  }
468  }
469 
470  return SUCCESSFUL_RETURN;
471 }
472 
473 
474 returnValue DenseMatrix_addToDiag( DenseMatrix* _THIS,real_t alpha )
475 {
476  int i;
477  for (i = 0; i < _THIS->nRows && i < _THIS->nCols; i++)
478  _THIS->val[i*((_THIS->leaDim)+1)] += alpha;
479 
480  return SUCCESSFUL_RETURN;
481 }
482 
483 
484 returnValue DenseMatrix_print( DenseMatrix* _THIS )
485 {
486  return qpOASES_printM( _THIS->val,_THIS->nRows,_THIS->nCols );
487 }
488 
489 
490 returnValue DenseMatrix_bilinear( DenseMatrix* _THIS,
491  const Indexlist* const icols, int xN, const real_t *x, int xLD, real_t *y, int yLD
492  )
493 {
494  int ii, jj, kk, col;
495  int i,j,k,irA;
496 
497  myStatic real_t Ax[NVCMAX*NVMAX];
498  real_t h;
499 
500  for (ii = 0; ii < xN; ii++)
501  for (jj = 0; jj < xN; jj++)
502  y[ii*yLD+jj] = 0.0;
503 
504  for (i=0;i<icols->length * xN;++i)
505  Ax[i]=0.0;
506 
507  /* exploit symmetry of A ! */
508  for (j = 0; j < icols->length; j++) {
509  irA = icols->number[j] * (_THIS->leaDim);
510  for (i = 0; i < icols->length; i++)
511  {
512  h = _THIS->val[irA+icols->number[i]];
513  for (k = 0; k < xN; k++)
514  Ax[j + k * icols->length] += h * x[k*xLD+icols->number[i]];
515  }
516  }
517 
518  for (ii = 0; ii < icols->length; ++ii) {
519  col = icols->number[ii];
520  for (jj = 0; jj < xN; ++jj) {
521  for (kk = 0; kk < xN; ++kk) {
522  y[kk + jj*yLD] += x[col + jj*xLD] * Ax[ii + kk*icols->length];
523  }
524  }
525  }
526 
527  return SUCCESSFUL_RETURN;
528 }
529 
530 
531 void dgemm_( const char *TRANSA, const char *TRANSB,
532  const unsigned long *M, const unsigned long *N, const unsigned long *K,
533  const double *ALPHA, const double *A, const unsigned long *LDA, const double *B, const unsigned long *LDB,
534  const double *BETA, double *C, const unsigned long *LDC
535  )
536 {
537  unsigned int i, j, k;
538 
539  if ( REFER_NAMESPACE_QPOASES qpOASES_isZero(*BETA,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
540  for (k = 0; k < *N; k++)
541  for (j = 0; j < *M; j++)
542  C[j+(*LDC)*k] = 0.0;
543  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*BETA,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
544  for (k = 0; k < *N; k++)
545  for (j = 0; j < *M; j++)
546  C[j+(*LDC)*k] = -C[j+(*LDC)*k];
547  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*BETA,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_FALSE )
548  for (k = 0; k < *N; k++)
549  for (j = 0; j < *M; j++)
550  C[j+(*LDC)*k] *= *BETA;
551 
552  if (TRANSA[0] == 'N')
553  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*ALPHA,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
554  for (k = 0; k < *N; k++)
555  for (j = 0; j < *M; j++)
556  for (i = 0; i < *K; i++)
557  C[j+(*LDC)*k] += A[j+(*LDA)*i] * B[i+(*LDB)*k];
558  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*ALPHA,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
559  for (k = 0; k < *N; k++)
560  for (j = 0; j < *M; j++)
561  for (i = 0; i < *K; i++)
562  C[j+(*LDC)*k] -= A[j+(*LDA)*i] * B[i+(*LDB)*k];
563  else
564  for (k = 0; k < *N; k++)
565  for (j = 0; j < *M; j++)
566  for (i = 0; i < *K; i++)
567  C[j+(*LDC)*k] += *ALPHA * A[j+(*LDA)*i] * B[i+(*LDB)*k];
568  else
569  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*ALPHA,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
570  for (k = 0; k < *N; k++)
571  for (j = 0; j < *M; j++)
572  for (i = 0; i < *K; i++)
573  C[j+(*LDC)*k] += A[i+(*LDA)*j] * B[i+(*LDB)*k];
574  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*ALPHA,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
575  for (k = 0; k < *N; k++)
576  for (j = 0; j < *M; j++)
577  for (i = 0; i < *K; i++)
578  C[j+(*LDC)*k] -= A[i+(*LDA)*j] * B[i+(*LDB)*k];
579  else
580  for (k = 0; k < *N; k++)
581  for (j = 0; j < *M; j++)
582  for (i = 0; i < *K; i++)
583  C[j+(*LDC)*k] += *ALPHA * A[i+(*LDA)*j] * B[i+(*LDB)*k];
584 }
585 
586 void sgemm_( const char *TRANSA, const char *TRANSB,
587  const unsigned long *M, const unsigned long *N, const unsigned long *K,
588  const float *ALPHA, const float *A, const unsigned long *LDA, const float *B, const unsigned long *LDB,
589  const float *BETA, float *C, const unsigned long *LDC
590  )
591 {
592  unsigned int i, j, k;
593 
594  if ( REFER_NAMESPACE_QPOASES qpOASES_isZero(*BETA,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
595  for (k = 0; k < *N; k++)
596  for (j = 0; j < *M; j++)
597  C[j+(*LDC)*k] = 0.0;
598  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*BETA,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
599  for (k = 0; k < *N; k++)
600  for (j = 0; j < *M; j++)
601  C[j+(*LDC)*k] = -C[j+(*LDC)*k];
602  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*BETA,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_FALSE )
603  for (k = 0; k < *N; k++)
604  for (j = 0; j < *M; j++)
605  C[j+(*LDC)*k] *= *BETA;
606 
607  if (TRANSA[0] == 'N')
608  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*ALPHA,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
609  for (k = 0; k < *N; k++)
610  for (j = 0; j < *M; j++)
611  for (i = 0; i < *K; i++)
612  C[j+(*LDC)*k] += A[j+(*LDA)*i] * B[i+(*LDB)*k];
613  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*ALPHA,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
614  for (k = 0; k < *N; k++)
615  for (j = 0; j < *M; j++)
616  for (i = 0; i < *K; i++)
617  C[j+(*LDC)*k] -= A[j+(*LDA)*i] * B[i+(*LDB)*k];
618  else
619  for (k = 0; k < *N; k++)
620  for (j = 0; j < *M; j++)
621  for (i = 0; i < *K; i++)
622  C[j+(*LDC)*k] += *ALPHA * A[j+(*LDA)*i] * B[i+(*LDB)*k];
623  else
624  if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*ALPHA,1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
625  for (k = 0; k < *N; k++)
626  for (j = 0; j < *M; j++)
627  for (i = 0; i < *K; i++)
628  C[j+(*LDC)*k] += A[i+(*LDA)*j] * B[i+(*LDB)*k];
629  else if ( REFER_NAMESPACE_QPOASES qpOASES_isEqual(*ALPHA,-1.0,QPOASES_TOL) == REFER_NAMESPACE_QPOASES BT_TRUE )
630  for (k = 0; k < *N; k++)
631  for (j = 0; j < *M; j++)
632  for (i = 0; i < *K; i++)
633  C[j+(*LDC)*k] -= A[i+(*LDA)*j] * B[i+(*LDB)*k];
634  else
635  for (k = 0; k < *N; k++)
636  for (j = 0; j < *M; j++)
637  for (i = 0; i < *K; i++)
638  C[j+(*LDC)*k] += *ALPHA * A[i+(*LDA)*j] * B[i+(*LDB)*k];
639 }
640 
641 
642 
643 void dpotrf_( const char *uplo, const unsigned long *_n, double *a,
644  const unsigned long *_lda, long *info
645  )
646 {
647  double sum;
648  long i, j, k;
649  long n = (long)(*_n);
650  long lda = (long)(*_lda);
651 
652  for( i=0; i<n; ++i )
653  {
654  /* j == i */
655  sum = a[i + lda*i];
656 
657  for( k=(i-1); k>=0; --k )
658  sum -= a[k+lda*i] * a[k+lda*i];
659 
660  if ( sum > 0.0 )
661  a[i+lda*i] = qpOASES_getSqrt( sum );
662  else
663  {
664  a[0] = sum; /* tunnel negative diagonal element to caller */
665  if (info != 0)
666  *info = (long)i+1;
667  return;
668  }
669 
670  for( j=(i+1); j<n; ++j )
671  {
672  sum = a[j*lda + i];
673 
674  for( k=(i-1); k>=0; --k )
675  sum -= a[k+lda*i] * a[k+lda*j];
676 
677  a[i+lda*j] = sum / a[i+lda*i];
678  }
679  }
680  if (info != 0)
681  *info = 0;
682 }
683 
684 
685 void spotrf_( const char *uplo, const unsigned long *_n, float *a,
686  const unsigned long *_lda, long *info
687  )
688 {
689  float sum;
690  long i, j, k;
691  long n = (long)(*_n);
692  long lda = (long)(*_lda);
693 
694  for( i=0; i<n; ++i )
695  {
696  /* j == i */
697  sum = a[i + lda*i];
698 
699  for( k=(i-1); k>=0; --k )
700  sum -= a[k+lda*i] * a[k+lda*i];
701 
702  if ( sum > 0.0 )
703  a[i+lda*i] = (float)(REFER_NAMESPACE_QPOASES qpOASES_getSqrt( sum ));
704  else
705  {
706  a[0] = sum; /* tunnel negative diagonal element to caller */
707  if (info != 0)
708  *info = (long)i+1;
709  return;
710  }
711 
712  for( j=(i+1); j<n; ++j )
713  {
714  sum = a[j*lda + i];
715 
716  for( k=(i-1); k>=0; --k )
717  sum -= a[k+lda*i] * a[k+lda*j];
718 
719  a[i+lda*j] = sum / a[i+lda*i];
720  }
721  }
722  if (info != 0)
723  *info = 0;
724 }
725 
726 
727 END_NAMESPACE_QPOASES
728 
729 
730 /*
731  * end of file
732  */
N
#define N
Definition: crane_kul_mhe_test.cpp:44
M
#define M
Definition: rocket_with_templates_ocp.hpp:43
qpOASES_isEqual
static BooleanType qpOASES_isEqual(real_t x, real_t y, real_t TOL)
Definition: Utils.h:371
DenseMatrix_getNorm
real_t DenseMatrix_getNorm(DenseMatrix *_THIS, int type)
Definition: Matrices.c:135
DenseMatrix_getCol
returnValue DenseMatrix_getCol(DenseMatrix *_THIS, int cNum, const Indexlist *const irows, real_t alpha, real_t *col)
Definition: Matrices.c:178
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
DenseMatrix_bilinear
returnValue DenseMatrix_bilinear(DenseMatrix *_THIS, const Indexlist *const icols, int xN, const real_t *x, int xLD, real_t *y, int yLD)
Definition: Matrices.c:490
DenseMatrix_getRowNorm
real_t DenseMatrix_getRowNorm(DenseMatrix *_THIS, int rNum, int type)
Definition: Matrices.c:141
DenseMatrix_subTimes
returnValue DenseMatrix_subTimes(DenseMatrix *_THIS, const Indexlist *const irows, const Indexlist *const icols, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD, BooleanType yCompr)
Definition: Matrices.c:230
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
NVMAX
#define NVMAX
Definition: qpoases3/include/qpOASES_e/Constants.h:70
DenseMatrixCON
BEGIN_NAMESPACE_QPOASES void DenseMatrixCON(DenseMatrix *_THIS, int m, int n, int lD, real_t *v)
Definition: Matrices.c:47
DenseMatrix_subTransTimes
returnValue DenseMatrix_subTransTimes(DenseMatrix *_THIS, const Indexlist *const irows, const Indexlist *const icols, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD)
Definition: Matrices.c:416
qpOASES_printM
returnValue qpOASES_printM(const real_t *const M, int nrow, int ncol)
Definition: Utils.c:147
DenseMatrix_print
returnValue DenseMatrix_print(DenseMatrix *_THIS)
Definition: Matrices.c:484
spotrf_
void spotrf_(const char *uplo, const unsigned long *_n, float *a, const unsigned long *_lda, long *info)
Definition: Matrices.c:685
qpOASES_isZero
static BooleanType qpOASES_isZero(real_t x, real_t TOL)
Definition: Utils.h:386
DenseMatrix_init
returnValue DenseMatrix_init(DenseMatrix *_THIS, int m, int n, int lD, real_t *v)
Definition: Matrices.c:80
DenseMatrixCPY
void DenseMatrixCPY(DenseMatrix *FROM, DenseMatrix *TO)
Definition: Matrices.c:58
DenseMatrix
Interfaces matrix-vector operations tailored to general dense matrices.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:273
DenseMatrix_isDiag
BooleanType DenseMatrix_isDiag(DenseMatrix *_THIS)
Definition: Matrices.c:119
BooleanType
BooleanType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:44
qpOASES_getMax
static real_t qpOASES_getMax(real_t x, real_t y)
Definition: Utils.h:436
dgemm_
void dgemm_(const char *TRANSA, const char *TRANSB, const unsigned long *M, const unsigned long *N, const unsigned long *K, const double *ALPHA, const double *A, const unsigned long *LDA, const double *B, const unsigned long *LDB, const double *BETA, double *C, const unsigned long *LDC)
Definition: Matrices.c:531
qpOASES_getSqrt
static real_t qpOASES_getSqrt(real_t x)
Definition: Utils.h:481
qpOASES_getNorm
real_t qpOASES_getNorm(const real_t *const v, int n, int type)
Definition: Utils.c:629
QPOASES_INFTY
static const real_t QPOASES_INFTY
Definition: qpoases3/include/qpOASES_e/Constants.h:114
v
#define v
END_NAMESPACE_QPOASES
#define END_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:120
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
row
RowXpr row(Index i)
Definition: BlockMethods.h:725
DenseMatrix_diag
real_t DenseMatrix_diag(DenseMatrix *_THIS, int i)
Definition: Matrices.c:108
DenseMatrix_transTimes
returnValue DenseMatrix_transTimes(DenseMatrix *_THIS, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD)
Definition: Matrices.c:213
QPOASES_EPS
static const real_t QPOASES_EPS
Definition: qpoases3/include/qpOASES_e/Constants.h:101
DenseMatrix_times
returnValue DenseMatrix_times(DenseMatrix *_THIS, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD)
Definition: Matrices.c:196
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
NVCMAX
#define NVCMAX
Definition: qpoases3/include/qpOASES_e/Constants.h:79
sgemm_
void sgemm_(const char *TRANSA, const char *TRANSB, const unsigned long *M, const unsigned long *N, const unsigned long *K, const float *ALPHA, const float *A, const unsigned long *LDA, const float *B, const unsigned long *LDB, const float *BETA, float *C, const unsigned long *LDC)
Definition: Matrices.c:586
col
ColXpr col(Index i)
Definition: BlockMethods.h:708
BEGIN_NAMESPACE_QPOASES
#define BEGIN_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:117
real_t
double real_t
Definition: AD_test.c:10
QPOASES_TOL
static const real_t QPOASES_TOL
Definition: qpoases3/include/qpOASES_e/Constants.h:118
DenseMatrix_getRow
returnValue DenseMatrix_getRow(DenseMatrix *_THIS, int rNum, const Indexlist *const icols, real_t alpha, real_t *row)
Definition: Matrices.c:147
DenseMatrix_free
void DenseMatrix_free(DenseMatrix *_THIS)
Definition: Matrices.c:75
dpotrf_
void dpotrf_(const char *uplo, const unsigned long *_n, double *a, const unsigned long *_lda, long *info)
Definition: Matrices.c:643
REFER_NAMESPACE_QPOASES
#define REFER_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:126
myStatic
#define myStatic
Definition: Types.h:103
DenseMatrix_addToDiag
returnValue DenseMatrix_addToDiag(DenseMatrix *_THIS, real_t alpha)
Definition: Matrices.c:474

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