qpOASES-3.2.0/src/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 
35 #include <qpOASES/Matrices.hpp>
36 
37 
38 BEGIN_NAMESPACE_QPOASES
39 
40 
41 
42 /*****************************************************************************
43  * P U B L I C *
44  *****************************************************************************/
45 
46 returnValue Matrix::getSparseSubmatrix(
47  const Indexlist* const irows,
48  const Indexlist* const icols,
49  int_t rowoffset,
50  int_t coloffset,
51  int_t& numNonzeros,
52  int_t* irn,
53  int_t* jcn,
54  real_t* avals,
55  BooleanType only_lower_triangular) const
56 {
57  int_t* rowsNumbers;
58  irows->getNumberArray( &rowsNumbers );
59  int_t* colsNumbers;
60  icols->getNumberArray( &colsNumbers );
61 
62  return getSparseSubmatrix(irows->getLength(), rowsNumbers, icols->getLength(), colsNumbers, rowoffset, coloffset, numNonzeros, irn, jcn, avals, only_lower_triangular);
63 }
64 
65 returnValue Matrix::getSparseSubmatrix(
66  const Indexlist* const irows,
67  int_t idx_icol,
68  int_t rowoffset,
69  int_t coloffset,
70  int_t& numNonzeros,
71  int_t* irn,
72  int_t* jcn,
73  real_t* avals,
74  BooleanType only_lower_triangular) const
75 {
76  int_t* rowsNumbers;
77  irows->getNumberArray( &rowsNumbers );
78 
79  return getSparseSubmatrix(irows->getLength(), rowsNumbers, 1, &idx_icol, rowoffset, coloffset, numNonzeros, irn, jcn, avals, only_lower_triangular);
80 }
81 
82 returnValue Matrix::getSparseSubmatrix(
83  int_t idx_row,
84  const Indexlist* const icols,
85  int_t rowoffset,
86  int_t coloffset,
87  int_t& numNonzeros,
88  int_t* irn,
89  int_t* jcn,
90  real_t* avals,
91  BooleanType only_lower_triangular) const
92 {
93  int_t* colsNumbers;
94  icols->getNumberArray( &colsNumbers );
95 
96  return getSparseSubmatrix(1, &idx_row, icols->getLength(), colsNumbers, rowoffset, coloffset, numNonzeros, irn, jcn, avals, only_lower_triangular);
97 }
98 
99 
100 DenseMatrix::~DenseMatrix()
101 {
102  if ( needToFreeMemory( ) == BT_TRUE )
103  free( );
104 }
105 
106 void DenseMatrix::free( )
107 {
108  if (val != 0)
109  delete[] val;
110  val = 0;
111 }
112 
113 Matrix *DenseMatrix::duplicate( ) const
114 {
115  DenseMatrix *dupl = 0;
116 
117  if ( needToFreeMemory( ) == BT_TRUE )
118  {
119  real_t* val_new = new real_t[nRows*nCols];
120  memcpy( val_new,val, ((uint_t)(nRows*nCols))*sizeof(real_t) );
121  dupl = new DenseMatrix(nRows, nCols, nCols, val_new);
122  dupl->doFreeMemory( );
123  }
124  else
125  {
126  dupl = new DenseMatrix(nRows, nCols, nCols, val);
127  }
128 
129  return dupl;
130 }
131 
132 real_t DenseMatrix::diag( int_t i
133  ) const
134 {
135  return val[i*(leaDim+1)];
136 }
137 
138 BooleanType DenseMatrix::isDiag( ) const
139 {
140  int_t i, j;
141 
142  if (nRows != nCols)
143  return BT_FALSE;
144 
145  for ( i=0; i<nRows; ++i )
146  for ( j=0; j<i; ++j )
147  if ( ( getAbs( val[i*leaDim+j] ) > EPS ) || ( getAbs( val[j*leaDim+i] ) > EPS ) )
148  return BT_FALSE;
149 
150  return BT_TRUE;
151 }
152 
153 
154 real_t DenseMatrix::getNorm( int_t type
155  ) const
156 {
157  return REFER_NAMESPACE_QPOASES getNorm( val,nCols*nRows,type );
158 }
159 
160 
161 real_t DenseMatrix::getRowNorm( int_t rNum, int_t type ) const
162 {
163  return REFER_NAMESPACE_QPOASES getNorm( &(val[rNum*leaDim]),nCols,type );
164 }
165 
166 returnValue DenseMatrix::getRow(int_t rNum, const Indexlist* const icols, real_t alpha, real_t *row) const
167 {
168  int_t i;
169  if (icols != 0)
170  {
171  if ( isEqual(alpha,1.0) == BT_TRUE )
172  for (i = 0; i < icols->length; i++)
173  row[i] = val[rNum*leaDim+icols->number[i]];
174  else if ( isEqual(alpha,-1.0) == BT_TRUE )
175  for (i = 0; i < icols->length; i++)
176  row[i] = -val[rNum*leaDim+icols->number[i]];
177  else
178  for (i = 0; i < icols->length; i++)
179  row[i] = alpha*val[rNum*leaDim+icols->number[i]];
180  }
181  else
182  {
183  if ( isEqual(alpha,1.0) == BT_TRUE )
184  for (i = 0; i < nCols; i++)
185  row[i] = val[rNum*leaDim+i];
186  else if ( isEqual(alpha,-1.0) == BT_TRUE )
187  for (i = 0; i < nCols; i++)
188  row[i] = -val[rNum*leaDim+i];
189  else
190  for (i = 0; i < nCols; i++)
191  row[i] = alpha*val[rNum*leaDim+i];
192  }
193  return SUCCESSFUL_RETURN;
194 }
195 
196 
197 returnValue DenseMatrix::getCol(int_t cNum, const Indexlist* const irows, real_t alpha, real_t *col) const
198 {
199  int_t i;
200 
201  if ( isEqual(alpha,1.0) == BT_TRUE )
202  for (i = 0; i < irows->length; i++)
203  col[i] = val[irows->number[i]*leaDim+cNum];
204  else if ( isEqual(alpha,-1.0) == BT_TRUE )
205  for (i = 0; i < irows->length; i++)
206  col[i] = -val[irows->number[i]*leaDim+cNum];
207  else
208  for (i = 0; i < irows->length; i++)
209  col[i] = alpha*val[irows->number[i]*leaDim+cNum];
210 
211  return SUCCESSFUL_RETURN;
212 }
213 
214 returnValue DenseMatrix::getSparseSubmatrix (int_t irowsLength, const int_t* const irowsNumber,
215  int_t icolsLength, const int_t* const icolsNumber,
216  int_t rowoffset, int_t coloffset, int_t& numNonzeros, int_t* irn,
217  int_t* jcn, real_t* avals,
218  BooleanType only_lower_triangular /*= BT_FALSE */) const
219 {
220  int_t i, j, irA;
221  real_t v;
222  numNonzeros = 0;
223  if ( only_lower_triangular == BT_FALSE )
224  {
225  if (irn == 0)
226  {
227  if (jcn != 0 || avals != 0)
228  return THROWERROR( RET_INVALID_ARGUMENTS );
229  for (j = 0; j<irowsLength; j++)
230  {
231  irA = irowsNumber[j] * leaDim;
232  for (i = 0; i<icolsLength; i++)
233  if (isZero( val[irA+icolsNumber[i]] ) == BT_FALSE)
234  numNonzeros++;
235  }
236  }
237  else
238  {
239  for (j = 0; j<irowsLength; j++)
240  {
241  irA = irowsNumber[j] * leaDim;
242  for (i = 0; i<icolsLength; i++)
243  {
244  v = val[irA+icolsNumber[i]];
245  if (isZero( v ) == BT_FALSE)
246  {
247  irn[numNonzeros] = j+rowoffset;
248  jcn[numNonzeros] = i+coloffset;
249  avals[numNonzeros] = v;
250  numNonzeros++;
251  }
252  }
253  }
254  }
255  }
256  else
257  {
258  if (irn == 0)
259  {
260  if (jcn != 0 || avals != 0)
261  return THROWERROR( RET_INVALID_ARGUMENTS );
262  for (j = 0; j<irowsLength; j++)
263  {
264  irA = irowsNumber[j] * leaDim;
265  for (i = 0; i<=j; i++)
266  if (isZero( val[irA+irowsNumber[i]] ) == BT_FALSE)
267  numNonzeros++;
268  }
269  }
270  else
271  {
272  for (j = 0; j<irowsLength; j++)
273  {
274  irA = irowsNumber[j] * leaDim;
275  for (i = 0; i<=j; i++)
276  {
277  v = val[irA+irowsNumber[i]];
278  if (isZero( v ) == BT_FALSE)
279  {
280  irn[numNonzeros] = j+rowoffset;
281  jcn[numNonzeros] = i+coloffset;
282  avals[numNonzeros] = v;
283  numNonzeros++;
284  }
285  }
286  }
287  }
288  }
289 
290  return SUCCESSFUL_RETURN;
291 }
292 
293 returnValue DenseMatrix::times( int_t xN, real_t alpha, const real_t *x, int_t xLD, real_t beta, real_t *y, int_t yLD) const
294 {
295  unsigned long _xN = (unsigned long)xN;
296  unsigned long _nRows = (unsigned long)nRows;
297  unsigned long _nCols = (unsigned long)nCols;
298  unsigned long _leaDim = (unsigned long)getMax(1,nCols);
299  unsigned long _xLD = (unsigned long)getMax(1,xLD);
300  unsigned long _yLD = (unsigned long)getMax(1,yLD);
301 
302  /* Call BLAS. Mind row major format! */
303  GEMM("TRANS", "NOTRANS", &_nRows, &_xN, &_nCols, &alpha, val, &_leaDim, x, &_xLD, &beta, y, &_yLD);
304  return SUCCESSFUL_RETURN;
305 }
306 
307 returnValue DenseMatrix::transTimes( int_t xN, real_t alpha, const real_t *x, int_t xLD, real_t beta, real_t *y, int_t yLD) const
308 {
309  unsigned long _xN = (unsigned long)xN;
310  unsigned long _nRows = (unsigned long)nRows;
311  unsigned long _nCols = (unsigned long)nCols;
312  unsigned long _leaDim = (unsigned long)getMax(1,nCols);
313  unsigned long _xLD = (unsigned long)getMax(1,xLD);
314  unsigned long _yLD = (unsigned long)getMax(1,yLD);
315 
316  /* Call BLAS. Mind row major format! */
317  GEMM("NOTRANS", "NOTRANS", &_nCols, &_xN, &_nRows, &alpha, val, &_leaDim, x, &_xLD, &beta, y, &_yLD);
318  return SUCCESSFUL_RETURN;
319 }
320 
321 returnValue DenseMatrix::times( const Indexlist* const irows, const Indexlist* const icols,
322  int_t xN, real_t alpha, const real_t *x, int_t xLD, real_t beta, real_t *y, int_t yLD,
323  BooleanType yCompr) const
324 {
325  int_t i, j, k, row, col, iy, irA;
326 
327  if (yCompr == BT_TRUE)
328  {
329  if ( isZero(beta) == BT_TRUE )
330  for (k = 0; k < xN; k++)
331  for (j = 0; j < irows->length; j++)
332  y[j+k*yLD] = 0.0;
333  else if ( isEqual(beta,-1.0) == BT_TRUE )
334  for (k = 0; k < xN; k++)
335  for (j = 0; j < irows->length; j++)
336  y[j+k*yLD] = -y[j+k*yLD];
337  else if ( isEqual(beta,1.0) == BT_FALSE )
338  for (k = 0; k < xN; k++)
339  for (j = 0; j < irows->length; j++)
340  y[j+k*yLD] *= beta;
341 
342  if (icols == 0)
343  if ( isEqual(alpha,1.0) == BT_TRUE )
344  for (k = 0; k < xN; k++)
345  for (j = 0; j < irows->length; j++)
346  {
347  row = irows->iSort[j];
348  iy = row + k * yLD;
349  irA = irows->number[row] * leaDim;
350  for (i = 0; i < nCols; i++)
351  y[iy] += val[irA+i] * x[k*xLD+i];
352  }
353  else if ( isEqual(alpha,-1.0) == BT_TRUE )
354  for (k = 0; k < xN; k++)
355  for (j = 0; j < irows->length; j++)
356  {
357  row = irows->iSort[j];
358  iy = row + k * yLD;
359  irA = irows->number[row] * leaDim;
360  for (i = 0; i < nCols; i++)
361  y[iy] -= val[irA+i] * x[k*xLD+i];
362  }
363  else
364  for (k = 0; k < xN; k++)
365  for (j = 0; j < irows->length; j++)
366  {
367  row = irows->iSort[j];
368  iy = row + k * yLD;
369  irA = irows->number[row] * leaDim;
370  for (i = 0; i < nCols; i++)
371  y[iy] += alpha * val[irA+i] * x[k*xLD+i];
372  }
373  else /* icols != 0 */
374  if ( isEqual(alpha,1.0) == BT_TRUE )
375  for (k = 0; k < xN; k++)
376  for (j = 0; j < irows->length; j++)
377  {
378  row = irows->iSort[j];
379  iy = row + k * yLD;
380  irA = irows->number[row] * leaDim;
381  for (i = 0; i < icols->length; i++)
382  {
383  col = icols->iSort[i];
384  y[iy] += val[irA+icols->number[col]] * x[k*xLD+col];
385  }
386  }
387  else if ( isEqual(alpha,-1.0) == BT_TRUE )
388  for (k = 0; k < xN; k++)
389  for (j = 0; j < irows->length; j++)
390  {
391  row = irows->iSort[j];
392  iy = row + k * yLD;
393  irA = irows->number[row] * leaDim;
394  for (i = 0; i < icols->length; i++)
395  {
396  col = icols->iSort[i];
397  y[iy] -= val[irA+icols->number[col]] * x[k*xLD+col];
398  }
399  }
400  else
401  for (k = 0; k < xN; k++)
402  for (j = 0; j < irows->length; j++)
403  {
404  row = irows->iSort[j];
405  iy = row + k * yLD;
406  irA = irows->number[row] * leaDim;
407  for (i = 0; i < icols->length; i++)
408  {
409  col = icols->iSort[i];
410  y[iy] += alpha * val[irA+icols->number[col]] * x[k*xLD+col];
411  }
412  }
413  }
414  else /* y not compressed */
415  {
416  if ( isZero(beta) == BT_TRUE )
417  for (k = 0; k < xN; k++)
418  for (j = 0; j < irows->length; j++)
419  y[irows->number[j]+k*yLD] = 0.0;
420  else if ( isEqual(beta,-1.0) == BT_TRUE )
421  for (k = 0; k < xN; k++)
422  for (j = 0; j < irows->length; j++)
423  y[irows->number[j]+k*yLD] = -y[j+k*yLD];
424  else if ( isEqual(beta,1.0) == BT_FALSE )
425  for (k = 0; k < xN; k++)
426  for (j = 0; j < irows->length; j++)
427  y[irows->number[j]+k*yLD] *= beta;
428 
429  if (icols == 0)
430  if ( isEqual(alpha,1.0) == BT_TRUE )
431  for (k = 0; k < xN; k++)
432  for (j = 0; j < irows->length; j++)
433  {
434  row = irows->number[irows->iSort[j]];
435  iy = row + k * yLD;
436  irA = row * leaDim;
437  for (i = 0; i < nCols; i++)
438  y[iy] += val[irA+i] * x[k*xLD+i];
439  }
440  else if ( isEqual(alpha,-1.0) == BT_TRUE )
441  for (k = 0; k < xN; k++)
442  for (j = 0; j < irows->length; j++)
443  {
444  row = irows->number[irows->iSort[j]];
445  iy = row + k * yLD;
446  irA = row * leaDim;
447  for (i = 0; i < nCols; i++)
448  y[iy] -= val[irA+i] * x[k*xLD+i];
449  }
450  else
451  for (k = 0; k < xN; k++)
452  for (j = 0; j < irows->length; j++)
453  {
454  row = irows->number[irows->iSort[j]];
455  iy = row + k * yLD;
456  irA = row * leaDim;
457  for (i = 0; i < nCols; i++)
458  y[iy] += alpha * val[irA+i] * x[k*xLD+i];
459  }
460  else /* icols != 0 */
461  if ( isEqual(alpha,1.0) == BT_TRUE )
462  for (k = 0; k < xN; k++)
463  for (j = 0; j < irows->length; j++)
464  {
465  row = irows->number[irows->iSort[j]];
466  iy = row + k * yLD;
467  irA = row * leaDim;
468  for (i = 0; i < icols->length; i++)
469  {
470  col = icols->iSort[i];
471  y[iy] += val[irA+icols->number[col]] * x[k*xLD+col];
472  }
473  }
474  else if ( isEqual(alpha,-1.0) == BT_TRUE )
475  for (k = 0; k < xN; k++)
476  for (j = 0; j < irows->length; j++)
477  {
478  row = irows->number[irows->iSort[j]];
479  iy = row + k * yLD;
480  irA = row * leaDim;
481  for (i = 0; i < icols->length; i++)
482  {
483  col = icols->iSort[i];
484  y[iy] -= val[irA+icols->number[col]] * x[k*xLD+col];
485  }
486  }
487  else
488  for (k = 0; k < xN; k++)
489  for (j = 0; j < irows->length; j++)
490  {
491  row = irows->number[irows->iSort[j]];
492  iy = row + k * yLD;
493  irA = row * leaDim;
494  for (i = 0; i < icols->length; i++)
495  {
496  col = icols->iSort[i];
497  y[iy] += alpha * val[irA+icols->number[col]] * x[k*xLD+col];
498  }
499  }
500  }
501 
502  return SUCCESSFUL_RETURN;
503 }
504 
505 returnValue DenseMatrix::transTimes( const Indexlist* const irows, const Indexlist* const icols,
506  int_t xN, real_t alpha, const real_t *x, int_t xLD, real_t beta, real_t *y, int_t yLD) const
507 {
508  int_t i, j, k, row, col;
509 
510  if ( isZero(beta) == BT_TRUE )
511  for (k = 0; k < xN; k++)
512  for (j = 0; j < icols->length; j++)
513  y[j+k*yLD] = 0.0;
514  else if ( isEqual(beta,-1.0) == BT_TRUE )
515  for (k = 0; k < xN; k++)
516  for (j = 0; j < icols->length; j++)
517  y[j+k*yLD] = -y[j+k*yLD];
518  else if ( isEqual(beta,1.0) == BT_FALSE )
519  for (k = 0; k < xN; k++)
520  for (j = 0; j < icols->length; j++)
521  y[j+k*yLD] *= beta;
522 
523  if ( isEqual(alpha,1.0) == BT_TRUE )
524  for (k = 0; k < xN; k++)
525  for (j = 0; j < irows->length; j++)
526  {
527  row = irows->iSort[j];
528  for (i = 0; i < icols->length; i++)
529  {
530  col = icols->iSort[i];
531  y[col+k*yLD] += val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
532  }
533  }
534  else if ( isEqual(alpha,-1.0) == BT_TRUE )
535  for (k = 0; k < xN; k++)
536  for (j = 0; j < irows->length; j++)
537  {
538  row = irows->iSort[j];
539  for (i = 0; i < icols->length; i++)
540  {
541  col = icols->iSort[i];
542  y[col+k*yLD] -= val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
543  }
544  }
545  else
546  for (k = 0; k < xN; k++)
547  for (j = 0; j < irows->length; j++)
548  {
549  row = irows->iSort[j];
550  for (i = 0; i < icols->length; i++)
551  {
552  col = icols->iSort[i];
553  y[col+k*yLD] += alpha * val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
554  }
555  }
556 
557  return SUCCESSFUL_RETURN;
558 }
559 
560 
561 returnValue DenseMatrix::addToDiag( real_t alpha )
562 {
563  int_t i;
564  for (i = 0; i < nRows && i < nCols; i++)
565  val[i*(leaDim+1)] += alpha;
566 
567  return SUCCESSFUL_RETURN;
568 }
569 
570 
571 returnValue DenseMatrix::writeToFile( FILE* output_file, const char* prefix ) const
572 {
573  return THROWERROR( RET_NOT_YET_IMPLEMENTED );
574 }
575 
576 
577 real_t* DenseMatrix::full() const
578 {
579  real_t* v = new real_t[nRows*nCols];
580  memcpy( v,val, ((uint_t)(nRows*nCols))*sizeof(real_t) );
581  return v;
582 }
583 
584 
585 returnValue DenseMatrix::print( const char* name ) const
586 {
587  return REFER_NAMESPACE_QPOASES print( val,nRows,nCols,name );
588 }
589 
590 
591 
592 Matrix *SymDenseMat::duplicate( ) const
593 {
594  return duplicateSym();
595 }
596 
597 
598 SymmetricMatrix *SymDenseMat::duplicateSym( ) const
599 {
600  /* "same" as duplicate() in DenseMatrix */
601  SymDenseMat *dupl = 0;
602 
603  if ( needToFreeMemory( ) == BT_TRUE )
604  {
605  real_t* val_new = new real_t[nRows*nCols];
606  memcpy( val_new,val, ((uint_t)(nRows*nCols))*sizeof(real_t) );
607  dupl = new SymDenseMat(nRows, nCols, nCols, val_new);
608  dupl->doFreeMemory( );
609  }
610  else
611  {
612  dupl = new SymDenseMat(nRows, nCols, nCols, val);
613  }
614 
615  return dupl;
616 }
617 
618 
619 returnValue SymDenseMat::bilinear( const Indexlist* const icols,
620  int_t xN, const real_t *x, int_t xLD, real_t *y, int_t yLD) const
621 {
622  int_t ii, jj, kk, col;
623  int_t i,j,k,irA;
624 
625  for (ii = 0; ii < xN; ii++)
626  for (jj = 0; jj < xN; jj++)
627  y[ii*yLD+jj] = 0.0;
628 
629  real_t *Ax = new real_t[icols->length * xN];
630 
631  for (i=0;i<icols->length * xN;++i)
632  Ax[i]=0.0;
633 
634  /* exploit symmetry of A ! */
635  for (j = 0; j < icols->length; j++) {
636  irA = icols->number[j] * leaDim;
637  for (i = 0; i < icols->length; i++)
638  {
639  real_t h = val[irA+icols->number[i]];
640  for (k = 0; k < xN; k++)
641  Ax[j + k * icols->length] += h * x[k*xLD+icols->number[i]];
642  }
643  }
644 
645  for (ii = 0; ii < icols->length; ++ii) {
646  col = icols->number[ii];
647  for (jj = 0; jj < xN; ++jj) {
648  for (kk = 0; kk < xN; ++kk) {
649  y[kk + jj*yLD] += x[col + jj*xLD] * Ax[ii + kk*icols->length];
650  }
651  }
652  }
653  delete[] Ax;
654 
655  return SUCCESSFUL_RETURN;
656 }
657 
658 
659 
660 SparseMatrix::SparseMatrix() : nRows(0), nCols(0), ir(0), jc(0), jd(0), val(0) {}
661 
662 SparseMatrix::SparseMatrix( int_t nr, int_t nc, sparse_int_t *r, sparse_int_t *c, real_t *v)
663  : nRows(nr), nCols(nc), ir(r), jc(c), jd(0), val(v) { doNotFreeMemory(); }
664 
665 SparseMatrix::SparseMatrix( int_t nr, int_t nc, int_t ld, const real_t * const v)
666  : nRows(nr), nCols(nc), jd(0)
667 {
668  int_t i, j, nnz;
669 
670  jc = new sparse_int_t[nc+1];
671  ir = new sparse_int_t[nr*nc];
672  val = new real_t[nr*nc];
673 
674  nnz = 0;
675  for (j = 0; j < nCols; j++)
676  {
677  jc[j] = nnz;
678  for (i = 0; i < nRows; i++)
679  if ( ( isZero( v[i*ld+j],0.0 ) == BT_FALSE ) || ( i == j ) ) /* also include zero diagonal elemets! */
680  {
681  ir[nnz] = i;
682  val[nnz++] = v[i*ld+j];
683  }
684  }
685  jc[nCols] = nnz;
686 
687  doFreeMemory( );
688 }
689 
690 
691 SparseMatrix::~SparseMatrix()
692 {
693  if (jd != 0)
694  {
695  delete[] jd;
696  jd = 0;
697  }
698 
699  if ( needToFreeMemory() == BT_TRUE )
700  free( );
701 }
702 
703 
704 void SparseMatrix::free( )
705 {
706  if (ir != 0) delete[] ir;
707  ir = 0;
708  if (jc != 0) delete[] jc;
709  jc = 0;
710  if (val != 0) delete[] val;
711  val = 0;
712 
713  doNotFreeMemory( );
714 }
715 
716 Matrix *SparseMatrix::duplicate() const
717 {
718  long i, length = jc[nCols];
719  SparseMatrix *dupl = new SparseMatrix;
720 
721  dupl->nRows = nRows;
722  dupl->nCols = nCols;
723  dupl->ir = new sparse_int_t[length];
724  dupl->jc = new sparse_int_t[nCols+1];
725  dupl->val = new real_t[length];
726 
727  for (i = 0; i < length; i++) dupl->ir[i] = ir[i];
728  for (i = 0; i <= nCols; i++) dupl->jc[i] = jc[i];
729  for (i = 0; i < length; i++) dupl->val[i] = val[i];
730 
731  if ( jd != 0 )
732  {
733  dupl->jd = new sparse_int_t[nCols];
734  for (i = 0; i < nCols; i++) dupl->jd[i] = jd[i];
735  }
736  else
737  dupl->jd = 0;
738 
739  dupl->doFreeMemory( );
740 
741  return dupl;
742 }
743 
744 
745 
746 real_t SparseMatrix::diag(int_t i) const
747 {
748  if ( jd == 0 )
749  {
750  THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
751  return INFTY;
752  }
753 
754  int_t entry = jd[i];
755  return (entry < jc[i+1] && ir[entry] == i) ? val[entry] : 0.0;
756 }
757 
758 
759 BooleanType SparseMatrix::isDiag() const
760 {
761  int_t j;
762 
763  if ( nCols != nRows )
764  return BT_FALSE;
765 
766  for (j = 0; j < nCols; ++j)
767  {
768  if ( jc[j+1] > jc[j]+1 )
769  return BT_FALSE;
770 
771  if ( ( jc[j+1] == jc[j]+1 ) && ( ir[jc[j]] != j ) )
772  return BT_FALSE;
773  }
774 
775  return BT_TRUE;
776 }
777 
778 
779 
780 real_t SparseMatrix::getNorm( int_t type
781  ) const
782 {
783  int_t length = jc[nCols];
784  return REFER_NAMESPACE_QPOASES getNorm( val,length,type );
785 }
786 
787 
788 real_t SparseMatrix::getRowNorm( int_t rNum, int_t type ) const
789 {
790  int_t i,j;
791  real_t norm = 0.0;
792 
793  switch( type )
794  {
795  case 2:
796  for ( j=0; j < nCols; ++j ) {
797  for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++) {};
798  norm += (i < jc[j+1] && ir[i] == rNum) ? val[i]*val[i] : 0.0;
799  }
800  return getSqrt(norm);
801 
802  case 1:
803  for ( j=0; j < nCols; ++j ) {
804  for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++) {};
805  norm += (i < jc[j+1] && ir[i] == rNum) ? REFER_NAMESPACE_QPOASES getAbs( val[i] ) : 0.0;
806  }
807  return norm;
808 
809  default:
810  THROWERROR( RET_INVALID_ARGUMENTS );
811  return -INFTY;
812  }
813 }
814 
815 
816 returnValue SparseMatrix::getRow(int_t rNum, const Indexlist* const icols, real_t alpha, real_t *row) const
817 {
818  long i, j, k;
819 
820  if (icols != 0)
821  {
822  if ( isEqual(alpha,1.0) == BT_TRUE )
823  for (k = 0; k < icols->length; k++)
824  {
825  j = icols->number[icols->iSort[k]];
826  for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
827  row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? val[i] : 0.0;
828  }
829  else if ( isEqual(alpha,-1.0) == BT_TRUE )
830  for (k = 0; k < icols->length; k++)
831  {
832  j = icols->number[icols->iSort[k]];
833  for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
834  row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? -val[i] : 0.0;
835  }
836  else
837  for (k = 0; k < icols->length; k++)
838  {
839  j = icols->number[icols->iSort[k]];
840  for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
841  row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? alpha*val[i] : 0.0;
842  }
843  }
844  else
845  {
846  if ( isEqual(alpha,1.0) == BT_TRUE )
847  for (j = 0; j < nCols; j++)
848  {
849  for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
850  row[j] = (i < jc[j+1] && ir[i] == rNum) ? val[i] : 0.0;
851  }
852  else if ( isEqual(alpha,-1.0) == BT_TRUE )
853  for (j = 0; j < icols->length; j++)
854  {
855  for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
856  row[j] = (i < jc[j+1] && ir[i] == rNum) ? -val[i] : 0.0;
857  }
858  else
859  for (j = 0; j < icols->length; j++)
860  {
861  for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
862  row[j] = (i < jc[j+1] && ir[i] == rNum) ? alpha*val[i] : 0.0;
863  }
864  }
865  return SUCCESSFUL_RETURN;
866 }
867 
868 
869 returnValue SparseMatrix::getCol(int_t cNum, const Indexlist* const irows, real_t alpha, real_t *col) const
870 {
871  long i, j;
872 
873  i = jc[cNum];
874  j = 0;
875  if ( isEqual(alpha,1.0) == BT_TRUE )
876  while (i < jc[cNum+1] && j < irows->length)
877  if (ir[i] == irows->number[irows->iSort[j]])
878  col[irows->iSort[j++]] = val[i++];
879  else if (ir[i] > irows->number[irows->iSort[j]])
880  col[irows->iSort[j++]] = 0.0;
881  else
882  i++;
883  else if ( isEqual(alpha,-1.0) == BT_TRUE )
884  while (i < jc[cNum+1] && j < irows->length)
885  if (ir[i] == irows->number[irows->iSort[j]])
886  col[irows->iSort[j++]] = -val[i++];
887  else if (ir[i] > irows->number[irows->iSort[j]])
888  col[irows->iSort[j++]] = 0.0;
889  else
890  i++;
891  else
892  while (i < jc[cNum+1] && j < irows->length)
893  if (ir[i] == irows->number[irows->iSort[j]])
894  col[irows->iSort[j++]] = alpha * val[i++];
895  else if (ir[i] > irows->number[irows->iSort[j]])
896  col[irows->iSort[j++]] = 0.0;
897  else
898  i++;
899 
900  /* fill in remaining zeros */
901  while (j < irows->length)
902  col[irows->iSort[j++]] = 0.0;
903 
904  return SUCCESSFUL_RETURN;
905 }
906 
907 returnValue SparseMatrix::getSparseSubmatrix (int_t irowsLength, const int_t* const irowsNumber,
908  int_t icolsLength, const int_t* const icolsNumber,
909  int_t rowoffset, int_t coloffset, int_t& numNonzeros, int_t* irn,
910  int_t* jcn, real_t* avals,
911  BooleanType only_lower_triangular /*= BT_FALSE */) const
912 {
913  int_t i, j, k, l;
914 
915  // Compute the "inverse" of the irows->number array
916  // TODO: Ideally this should be a part of Indexlist
917  int_t* rowNumberInv = new int_t[nRows];
918  for (i=0; i<nRows; i++)
919  rowNumberInv[i] = -1;
920  for (i=0; i<irowsLength; i++)
921  rowNumberInv[irowsNumber[i]] = i;
922 
923  numNonzeros = 0;
924  if ( only_lower_triangular == BT_FALSE )
925  {
926  if (irn == 0)
927  {
928  if (jcn != 0 || avals != 0)
929  return THROWERROR( RET_INVALID_ARGUMENTS );
930  for (k = 0; k < icolsLength; k++)
931  {
932  j = icolsNumber[k];
933  for (i = jc[j]; i < jc[j+1]; i++)
934  {
935  l = rowNumberInv[ir[i]];
936  if (l >= 0)
937  numNonzeros++;
938  }
939  }
940  }
941  else
942  {
943  for (k = 0; k < icolsLength; k++)
944  {
945  j = icolsNumber[k];
946  for (i = jc[j]; i < jc[j+1]; i++)
947  {
948  l = rowNumberInv[ir[i]];
949  if (l >= 0)
950  {
951  irn[numNonzeros] = l+rowoffset;
952  jcn[numNonzeros] = k+coloffset;
953  avals[numNonzeros] = val[i];
954  numNonzeros++;
955  }
956  }
957  }
958  }
959  }
960  else
961  {
962  if (irn == 0)
963  {
964  if (jcn != 0 || avals != 0)
965  return THROWERROR( RET_INVALID_ARGUMENTS );
966  for (k = 0; k < icolsLength; k++)
967  {
968  j = icolsNumber[k];
969  for (i = jc[j]; i < jc[j+1]; i++)
970  {
971  l = rowNumberInv[ir[i]];
972  if (l >= k)
973  numNonzeros++;
974  }
975  }
976  }
977  else
978  {
979  for (k = 0; k < icolsLength; k++)
980  {
981  j = icolsNumber[k];
982  for (i = jc[j]; i < jc[j+1]; i++)
983  {
984  l = rowNumberInv[ir[i]];
985  if (l >= k)
986  {
987  irn[numNonzeros] = l+rowoffset;
988  jcn[numNonzeros] = k+coloffset;
989  avals[numNonzeros] = val[i];
990  numNonzeros++;
991  }
992  }
993  }
994  }
995  }
996  delete [] rowNumberInv;
997 
998  return SUCCESSFUL_RETURN;
999 }
1000 
1001 returnValue SparseMatrix::times(int_t xN, real_t alpha, const real_t *x, int_t xLD,
1002  real_t beta, real_t *y, int_t yLD) const
1003 {
1004  long i, j, k;
1005 
1006  if ( isZero(beta) == BT_TRUE )
1007  for (k = 0; k < xN; k++)
1008  for (j = 0; j < nRows; j++)
1009  y[j+k*yLD] = 0.0;
1010  else if ( isEqual(beta,-1.0) == BT_TRUE )
1011  for (k = 0; k < xN; k++)
1012  for (j = 0; j < nRows; j++)
1013  y[j+k*yLD] = -y[j+k*yLD];
1014  else if ( isEqual(beta,1.0) == BT_FALSE )
1015  for (k = 0; k < xN; k++)
1016  for (j = 0; j < nRows; j++)
1017  y[j+k*yLD] *= beta;
1018 
1019  if ( isEqual(alpha,1.0) == BT_TRUE )
1020  for (k = 0; k < xN; k++)
1021  for (j = 0; j < nCols; j++)
1022  for (i = jc[j]; i < jc[j+1]; i++)
1023  y[ir[i]+k*yLD] += val[i] * x[j+k*xLD];
1024  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1025  for (k = 0; k < xN; k++)
1026  for (j = 0; j < nCols; j++)
1027  for (i = jc[j]; i < jc[j+1]; i++)
1028  y[ir[i]+k*yLD] -= val[i] * x[j+k*xLD];
1029  else
1030  for (k = 0; k < xN; k++)
1031  for (j = 0; j < nCols; j++)
1032  for (i = jc[j]; i < jc[j+1]; i++)
1033  y[ir[i]+k*yLD] += alpha * val[i] * x[j+k*xLD];
1034 
1035  return SUCCESSFUL_RETURN;
1036 }
1037 
1038 
1039 returnValue SparseMatrix::transTimes(int_t xN, real_t alpha, const real_t *x, int_t xLD,
1040  real_t beta, real_t *y, int_t yLD) const
1041 {
1042  long i, j, k;
1043 
1044  if ( isZero(beta) == BT_TRUE )
1045  for (k = 0; k < xN; k++)
1046  for (j = 0; j < nCols; j++)
1047  y[j+k*yLD] = 0.0;
1048  else if ( isEqual(beta,-1.0) == BT_TRUE )
1049  for (k = 0; k < xN; k++)
1050  for (j = 0; j < nCols; j++)
1051  y[j+k*yLD] = -y[j+k*yLD];
1052  else if ( isEqual(beta,1.0) == BT_FALSE )
1053  for (k = 0; k < xN; k++)
1054  for (j = 0; j < nCols; j++)
1055  y[j+k*yLD] *= beta;
1056 
1057  if ( isEqual(alpha,1.0) == BT_TRUE )
1058  for (k = 0; k < xN; k++)
1059  for (j = 0; j < nCols; j++)
1060  for (i = jc[j]; i < jc[j+1]; i++)
1061  y[j+k*yLD] += val[i] * x[ir[i]+k*xLD];
1062  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1063  for (k = 0; k < xN; k++)
1064  for (j = 0; j < nCols; j++)
1065  for (i = jc[j]; i < jc[j+1]; i++)
1066  y[j+k*yLD] -= val[i] * x[ir[i]+k*xLD];
1067  else
1068  for (k = 0; k < xN; k++)
1069  for (j = 0; j < nCols; j++)
1070  for (i = jc[j]; i < jc[j+1]; i++)
1071  y[j+k*yLD] += alpha * val[i] * x[ir[i]+k*xLD];
1072 
1073  return SUCCESSFUL_RETURN;
1074 }
1075 
1076 
1077 returnValue SparseMatrix::times(const Indexlist* const irows, const Indexlist* const icols,
1078  int_t xN, real_t alpha, const real_t *x, int_t xLD, real_t beta, real_t *y, int_t yLD,
1079  BooleanType yCompr) const
1080 {
1081  long i, j, k, l, col;
1082  real_t xcol;
1083 
1084  if ( isEqual(alpha,0.0) == BT_TRUE )
1085  {
1086  if (yCompr == BT_TRUE)
1087  {
1088  if ( isZero(beta) == BT_TRUE )
1089  for (k = 0; k < xN; k++)
1090  for (j = 0; j < irows->length; j++)
1091  y[j+k*yLD] = 0.0;
1092  else if ( isEqual(beta,-1.0) == BT_TRUE )
1093  for (k = 0; k < xN; k++)
1094  for (j = 0; j < irows->length; j++)
1095  y[j+k*yLD] = -y[j+k*yLD];
1096  else if ( isEqual(beta,1.0) == BT_FALSE )
1097  for (k = 0; k < xN; k++)
1098  for (j = 0; j < irows->length; j++)
1099  y[j+k*yLD] *= beta;
1100  }
1101  else
1102  {
1103  if (isZero( beta ) == BT_TRUE)
1104  for (k = 0; k < xN; k++)
1105  for (j = 0; j < irows->length; j++)
1106  y[irows->number[j]+k*yLD] = 0.0;
1107  else if (isEqual( beta, -1.0 ) == BT_TRUE)
1108  for (k = 0; k < xN; k++)
1109  for (j = 0; j < irows->length; j++)
1110  y[irows->number[j]+k*yLD] = -y[irows->number[j]+k*yLD];
1111  else if (isEqual( beta, 1.0 ) == BT_FALSE)
1112  for (k = 0; k < xN; k++)
1113  for (j = 0; j < irows->length; j++)
1114  y[irows->number[j]+k*yLD] *= beta;
1115  }
1116  return SUCCESSFUL_RETURN;
1117  }
1118 
1119  // First, work with full, unordered copy of y and store matrix times x in there
1120  const int_t yfullLength = nRows;
1121  real_t* ytmp = new real_t[xN*yfullLength];
1122  for (k = 0; k < xN*yfullLength; k++)
1123  ytmp[k] = 0.0;
1124 
1125  if (icols!=0)
1126  {
1127  if (xN==1)
1128  {
1129  for (l = 0; l < icols->length; l++)
1130  {
1131  col = icols->iSort[l];
1132  xcol = x[col];
1133  if (isZero( xcol ) == BT_FALSE)
1134  {
1135  j = icols->number[col];
1136  for (i = jc[j]; i < jc[j+1]; i++)
1137  ytmp[ir[i]] += val[i] * xcol;
1138  }
1139  }
1140  }
1141  else
1142  {
1143  // AW: I didn't test the case xN>1, but I hope it is working
1144  real_t* xcols = new real_t[xN];
1145  for (l = 0; l < icols->length; l++)
1146  {
1147  col = icols->iSort[l];
1148  real_t xmax = 0.0;
1149  for (k=0; k<xN; k++)
1150  {
1151  xcols[k] = x[k*xLD+col];
1152  xmax = getMax(xmax,getAbs(xcols[k]));
1153  }
1154  if (isZero( xmax ) == BT_FALSE)
1155  {
1156  j = icols->number[col];
1157  for (i = jc[j]; i < jc[j+1]; i++)
1158  for (k=0; k<xN; k++)
1159  // AW: Maybe it makes more sense to order ytmp by vectors, not vector entries, for better cache peformance?
1160  ytmp[k*yfullLength+ir[i]] += val[i] * xcols[k];
1161  }
1162  }
1163  delete [] xcols;
1164  }
1165  }
1166  else /* icols == 0 */
1167  {
1168  if (xN==1)
1169  {
1170  for (col = 0; col < nCols; col++)
1171  {
1172  xcol = x[col];
1173  if (isZero( xcol ) == BT_FALSE)
1174  for (i = jc[col]; i < jc[col+1]; i++)
1175  ytmp[ir[i]] += val[i] * xcol;
1176  }
1177  }
1178  else
1179  {
1180  // AW: I didn't test the case xN>1, but I hope it is working
1181  real_t* xcols = new real_t[xN];
1182  for (col = 0; col < nCols; col++)
1183  {
1184  real_t xmax = 0.0;
1185  for (k=0; k<xN; k++)
1186  {
1187  xcols[k] = x[k*xLD+col];
1188  xmax = getMax(xmax,getAbs(xcols[k]));
1189  }
1190  if (isZero( xmax ) == BT_FALSE)
1191  for (i = jc[col]; i < jc[col+1]; i++)
1192  for (k=0; k<xN; k++)
1193  ytmp[k*yfullLength+ir[i]] += val[i] * xcols[k];
1194  delete [] xcols;
1195  }
1196  }
1197  }
1198 
1199  if (yCompr == BT_TRUE)
1200  {
1201  if ( isZero(beta) == BT_TRUE )
1202  for (k = 0; k < xN; k++)
1203  for (j = 0; j < irows->length; j++)
1204  y[j+k*yLD] = alpha*ytmp[irows->number[j]+k*yfullLength];
1205  else if (isEqual( beta, 1.0 ) == BT_TRUE)
1206  for (k = 0; k < xN; k++)
1207  for (j = 0; j < irows->length; j++)
1208  y[j+k*yLD] += alpha*ytmp[irows->number[j]+k*yfullLength];
1209  else if (isEqual( beta, -1.0 ) == BT_TRUE)
1210  for (k = 0; k < xN; k++)
1211  for (j = 0; j < irows->length; j++)
1212  y[j+k*yLD] = alpha*ytmp[irows->number[j]+k*yfullLength]-y[j+k*yLD];
1213  else if (isEqual( beta, 1.0 ) == BT_FALSE)
1214  for (k = 0; k < xN; k++)
1215  for (j = 0; j < irows->length; j++)
1216  y[j+k*yLD] = alpha*ytmp[irows->number[j]+k*yfullLength]+beta*y[j+k*yLD];
1217  }
1218  else
1219  {
1220  if (isZero( beta ) == BT_TRUE)
1221  for (k = 0; k < xN; k++)
1222  for (j = 0; j < irows->length; j++)
1223  y[irows->number[j]+k*yLD] = alpha*ytmp[irows->number[j]+k*yfullLength];
1224  else if (isEqual( beta, 1.0 ) == BT_TRUE)
1225  for (k = 0; k < xN; k++)
1226  for (j = 0; j < irows->length; j++)
1227  y[irows->number[j]+k*yLD] = alpha*ytmp[irows->number[j]+k*yfullLength]+y[j+k*yLD];
1228  else if (isEqual( beta, -1.0 ) == BT_TRUE)
1229  for (k = 0; k < xN; k++)
1230  for (j = 0; j < irows->length; j++)
1231  y[irows->number[j]+k*yLD] = alpha*ytmp[irows->number[j]+k*yfullLength]-y[j+k*yLD];
1232  else if (isEqual( beta, 1.0 ) == BT_FALSE)
1233  for (k = 0; k < xN; k++)
1234  for (j = 0; j < irows->length; j++)
1235  y[irows->number[j]+k*yLD] = alpha*ytmp[irows->number[j]+k*yfullLength]+beta*y[j+k*yLD];
1236  }
1237 
1238  delete [] ytmp;
1239  return SUCCESSFUL_RETURN;
1240 }
1241 
1242 
1243 returnValue SparseMatrix::transTimes(const Indexlist* const irows, const Indexlist* const icols,
1244  int_t xN, real_t alpha, const real_t *x, int_t xLD, real_t beta, real_t *y, int_t yLD) const
1245 {
1246  long i, j, k, l, col;
1247  real_t yadd;
1248 
1249  if ( isZero(beta) == BT_TRUE )
1250  for (k = 0; k < xN; k++)
1251  for (j = 0; j < icols->length; j++)
1252  y[j+k*yLD] = 0.0;
1253  else if ( isEqual(beta,-1.0) == BT_TRUE )
1254  for (k = 0; k < xN; k++)
1255  for (j = 0; j < icols->length; j++)
1256  y[j+k*yLD] = -y[j+k*yLD];
1257  else if ( isEqual(beta,1.0) == BT_FALSE )
1258  for (k = 0; k < xN; k++)
1259  for (j = 0; j < icols->length; j++)
1260  y[j+k*yLD] *= beta;
1261  if ( isEqual(alpha,0.0) == BT_TRUE )
1262  return SUCCESSFUL_RETURN;
1263 
1264  // work with full, unordered copy of x
1265  const int_t xfullLength = nRows;
1266  real_t* xtmp = new real_t[xfullLength];
1267  for (k = 0; k < xN; k++)
1268  {
1269  for (i = 0; i < xfullLength; i++)
1270  xtmp[i] = 0.0;
1271  for (i = 0; i < irows->length; i++)
1272  xtmp[irows->number[i]] = x[k*xLD+i];
1273  for (l = 0; l < icols->length; l++)
1274  {
1275  col = icols->iSort[l];
1276  yadd = 0.0;
1277  j = icols->number[col];
1278  for (i = jc[j]; i < jc[j+1]; i++)
1279  yadd += val[i] * xtmp[ir[i]];
1280  y[col] += alpha*yadd;
1281  }
1282  y += yLD; // move on to next RHS
1283  }
1284 
1285  delete [] xtmp;
1286 
1287  return SUCCESSFUL_RETURN;
1288 }
1289 
1290 
1291 
1292 returnValue SparseMatrix::addToDiag(real_t alpha)
1293 {
1294  long i;
1295 
1296  if ( jd == 0 )
1297  return THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
1298 
1299  if ( isZero( alpha ) == BT_FALSE )
1300  {
1301  for (i = 0; i < nRows && i < nCols; i++)
1302  {
1303  if (ir[jd[i]] == i)
1304  val[jd[i]] += alpha;
1305  else
1306  return RET_NO_DIAGONAL_AVAILABLE;
1307  }
1308  }
1309 
1310  return SUCCESSFUL_RETURN;
1311 }
1312 
1313 
1314 sparse_int_t *SparseMatrix::createDiagInfo()
1315 {
1316  sparse_int_t i, j;
1317 
1318  if (jd == 0) {
1319  jd = new sparse_int_t[nCols];
1320 
1321  for (j = 0; j < nCols; j++)
1322  {
1323  for (i = jc[j]; i < jc[j+1] && ir[i] < j; i++);
1324  jd[j] = i;
1325  }
1326  }
1327 
1328  return jd;
1329 }
1330 
1331 
1332 
1333 real_t *SparseMatrix::full() const
1334 {
1335  sparse_int_t i, j;
1336  real_t *v = new real_t[nRows*nCols];
1337 
1338  for (i = 0; i < nCols*nRows; i++)
1339  v[i] = 0.0;
1340 
1341  for (j = 0; j < nCols; j++)
1342  for (i = jc[j]; i < jc[j+1]; i++)
1343  v[ir[i] * nCols + j] = val[i];
1344 
1345  return v;
1346 }
1347 
1348 
1349 returnValue SparseMatrix::print( const char* name ) const
1350 {
1351  real_t* tmp = this->full();
1352  returnValue retVal = REFER_NAMESPACE_QPOASES print( tmp,nRows,nCols,name );
1353  delete[] tmp;
1354 
1355  return retVal;
1356 }
1357 
1358 returnValue SparseMatrix::writeToFile( FILE* output_file, const char* prefix ) const
1359 {
1360  for (int_t i=0; i<=nCols; i++) {
1361  fprintf( output_file,"%sjc[%d] = %d\n",prefix,(int)i,(int)(jc[i]) );
1362  }
1363  for (int_t i=0; i<jc[nCols]; i++) {
1364  fprintf( output_file,"%sir[%d] = %d\n",prefix,(int)i,(int)(ir[i]) );
1365  }
1366  for (int_t i=0; i<jc[nCols]; i++) {
1367  fprintf( output_file,"%sval[%d] = %23.16e\n",prefix,(int)i,val[i] );
1368  }
1369 
1370  return SUCCESSFUL_RETURN;
1371 }
1372 
1373 
1374 SparseMatrixRow::SparseMatrixRow() : nRows(0), nCols(0), jr(0), ic(0), jd(0), val(0) {}
1375 
1376 SparseMatrixRow::SparseMatrixRow(int_t nr, int_t nc, sparse_int_t *r, sparse_int_t *c, real_t *v)
1377  : nRows(nr), nCols(nc), jr(r), ic(c), jd(0), val(v) { doNotFreeMemory(); }
1378 
1379 SparseMatrixRow::SparseMatrixRow(int_t nr, int_t nc, int_t ld, const real_t * const v) : nRows(nr), nCols(nc), jd(0)
1380 {
1381  int_t i, j, nnz;
1382 
1383  jr = new sparse_int_t[nr+1];
1384  ic = new sparse_int_t[nr*nc];
1385  val = new real_t[nr*nc];
1386 
1387  nnz = 0;
1388  for (j = 0; j < nRows; j++)
1389  {
1390  jr[j] = nnz;
1391  for (i = 0; i < nCols; i++)
1392  if ( ( isZero( v[j*ld+i],0.0 ) == BT_FALSE ) || ( j == i ) )
1393  {
1394  ic[nnz] = i;
1395  val[nnz++] = v[j*ld+i];
1396  }
1397  }
1398  jr[nRows] = nnz;
1399 
1400  doFreeMemory( );
1401 }
1402 
1403 
1404 SparseMatrixRow::~SparseMatrixRow()
1405 {
1406  if (jd != 0)
1407  {
1408  delete[] jd;
1409  jd = 0;
1410  }
1411 
1412  if ( needToFreeMemory() == BT_TRUE )
1413  free( );
1414 }
1415 
1416 
1417 void SparseMatrixRow::free( )
1418 {
1419  if (jr != 0) delete[] jr;
1420  jr = 0;
1421  if (ic != 0) delete[] ic;
1422  ic = 0;
1423  if (val != 0) delete[] val;
1424  val = 0;
1425 
1426  doNotFreeMemory( );
1427 }
1428 
1429 
1430 Matrix *SparseMatrixRow::duplicate() const
1431 {
1432  long i, length = jr[nRows];
1433  SparseMatrixRow *dupl = new SparseMatrixRow;
1434 
1435  dupl->nRows = nRows;
1436  dupl->nCols = nCols;
1437  dupl->jr = new sparse_int_t[nRows+1];
1438  dupl->ic = new sparse_int_t[length];
1439  dupl->val = new real_t[length];
1440 
1441  for (i = 0; i < length; i++) dupl->jr[i] = jr[i];
1442  for (i = 0; i <= nCols; i++) dupl->ic[i] = ic[i];
1443  for (i = 0; i < length; i++) dupl->val[i] = val[i];
1444 
1445  if ( jd != 0 )
1446  {
1447  dupl->jd = new sparse_int_t[nRows];
1448  for (i = 0; i < nCols; i++) dupl->jd[i] = jd[i];
1449  }
1450  else
1451  dupl->jd = 0;
1452 
1453  dupl->doFreeMemory( );
1454 
1455  return dupl;
1456 }
1457 
1458 
1459 
1460 real_t SparseMatrixRow::diag(int_t i) const
1461 {
1462  if ( jd == 0 )
1463  {
1464  THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
1465  return INFTY;
1466  }
1467 
1468  int_t entry = jd[i];
1469  return (entry < jr[i+1] && ic[entry] == i) ? val[entry] : 0.0;
1470 }
1471 
1472 
1473 BooleanType SparseMatrixRow::isDiag() const
1474 {
1475  int_t i;
1476 
1477  if ( nCols != nRows )
1478  return BT_FALSE;
1479 
1480  for (i = 0; i < nRows; ++i)
1481  {
1482  if ( jr[i+1] > jr[i]+1 )
1483  return BT_FALSE;
1484 
1485  if ( ( jr[i+1] == jr[i]+1 ) && ( ic[jr[i]] != i ) )
1486  return BT_FALSE;
1487  }
1488 
1489  return BT_TRUE;
1490 }
1491 
1492 
1493 
1494 real_t SparseMatrixRow::getNorm( int_t type
1495  ) const
1496 {
1497  int_t length = jr[nRows];
1498  return REFER_NAMESPACE_QPOASES getNorm( val,length,type );
1499 
1500 }
1501 
1502 
1503 real_t SparseMatrixRow::getRowNorm( int_t rNum, int_t type ) const
1504 {
1505  int_t length = jr[rNum+1] - jr[rNum];
1506  return REFER_NAMESPACE_QPOASES getNorm( &(val[jr[rNum]]),length,type );
1507 }
1508 
1509 
1510 
1511 returnValue SparseMatrixRow::getRow(int_t rNum, const Indexlist* const icols, real_t alpha, real_t *row) const
1512 {
1513  long i, j;
1514 
1515  if (icols != 0)
1516  {
1517  j = jr[rNum];
1518  i = 0;
1519  if ( isEqual(alpha,1.0) == BT_TRUE )
1520  while (j < jr[rNum+1] && i < icols->length)
1521  if (ic[j] == icols->number[icols->iSort[i]])
1522  row[icols->iSort[i++]] = val[j++];
1523  else if (ic[j] > icols->number[icols->iSort[i]])
1524  row[icols->iSort[i++]] = 0.0;
1525  else
1526  j++;
1527  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1528  while (j < jr[rNum+1] && i < icols->length)
1529  if (ic[j] == icols->number[icols->iSort[i]])
1530  row[icols->iSort[i++]] = -val[j++];
1531  else if (ic[j] > icols->number[icols->iSort[i]])
1532  row[icols->iSort[i++]] = 0.0;
1533  else
1534  j++;
1535  else
1536  while (j < jr[rNum+1] && i < icols->length)
1537  if (ic[j] == icols->number[icols->iSort[i]])
1538  row[icols->iSort[i++]] = alpha * val[j++];
1539  else if (ic[j] > icols->number[icols->iSort[i]])
1540  row[icols->iSort[i++]] = 0.0;
1541  else
1542  j++;
1543 
1544  /* fill in remaining zeros */
1545  while (i < icols->length)
1546  row[icols->iSort[i++]] = 0.0;
1547  }
1548  else
1549  {
1550  for (i = 0; i < nCols; i++)
1551  row[i] = 0;
1552 
1553  if ( isEqual(alpha,1.0) == BT_TRUE )
1554  for (j = jr[rNum]; j < jr[rNum+1]; j++)
1555  row[ic[j]] = val[j];
1556  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1557  for (j = jr[rNum]; j < jr[rNum+1]; j++)
1558  row[ic[j]] = -val[j];
1559  else
1560  for (j = jr[rNum]; j < jr[rNum+1]; j++)
1561  row[ic[j]] = alpha * val[j];
1562  }
1563 
1564  return SUCCESSFUL_RETURN;
1565 }
1566 
1567 
1568 returnValue SparseMatrixRow::getCol(int_t cNum, const Indexlist* const irows, real_t alpha, real_t *col) const
1569 {
1570  long i, j, k, srt;
1571 
1572  if (irows != 0)
1573  {
1574  if ( isEqual(alpha,1.0) == BT_TRUE )
1575  for (k = 0; k < irows->length; k++)
1576  {
1577  srt = irows->iSort[k];
1578  j = irows->number[srt];
1579  for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
1580  col[srt] = (i < jr[j+1] && ic[i] == cNum) ? val[i] : 0.0;
1581  }
1582  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1583  for (k = 0; k < irows->length; k++)
1584  {
1585  srt = irows->iSort[k];
1586  j = irows->number[srt];
1587  for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
1588  col[srt] = (i < jr[j+1] && ic[i] == cNum) ? -val[i] : 0.0;
1589  }
1590  else
1591  for (k = 0; k < irows->length; k++)
1592  {
1593  srt = irows->iSort[k];
1594  j = irows->number[srt];
1595  for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
1596  col[srt] = (i < jr[j+1] && ic[i] == cNum) ? alpha*val[i] : 0.0;
1597  }
1598  }
1599  else
1600  {
1601  if ( isEqual(alpha,1.0) == BT_TRUE )
1602  for (j = 0; j < nCols; j++)
1603  {
1604  for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
1605  col[j] = (i < jr[j+1] && ic[i] == cNum) ? val[i] : 0.0;
1606  }
1607  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1608  for (j = 0; j < irows->length; j++)
1609  {
1610  for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
1611  col[j] = (i < jr[j+1] && ic[i] == cNum) ? -val[i] : 0.0;
1612  }
1613  else
1614  for (j = 0; j < irows->length; j++)
1615  {
1616  for (i = jr[j]; i < jr[j+1] && ic[i] < cNum; i++);
1617  col[j] = (i < jr[j+1] && ic[i] == cNum) ? alpha*val[i] : 0.0;
1618  }
1619  }
1620  return SUCCESSFUL_RETURN;
1621 }
1622 
1623 returnValue SparseMatrixRow::getSparseSubmatrix (
1624  int_t irowsLength, const int_t* const irowsNumber,
1625  int_t icolsLength, const int_t* const icolsNumber,
1626  int_t rowoffset, int_t coloffset, int_t& numNonzeros, int_t* irn,
1627  int_t* jcn, real_t* avals, BooleanType only_lower_triangular /*= BT_FALSE */) const
1628 {
1629  fprintf(stderr, "SparseMatrixRow::getSparseSubmatrix not implemented!\n");
1630 
1631  return THROWERROR(RET_NOT_YET_IMPLEMENTED);
1632 }
1633 
1634 returnValue SparseMatrixRow::times(int_t xN, real_t alpha, const real_t *x, int_t xLD,
1635  real_t beta, real_t *y, int_t yLD) const
1636 {
1637  long i, j, k;
1638 
1639  if ( isZero(beta) == BT_TRUE )
1640  for (k = 0; k < xN; k++)
1641  for (j = 0; j < nRows; j++)
1642  y[j+k*yLD] = 0.0;
1643  else if ( isEqual(beta,-1.0) == BT_TRUE )
1644  for (k = 0; k < xN; k++)
1645  for (j = 0; j < nRows; j++)
1646  y[j+k*yLD] = -y[j+k*yLD];
1647  else if ( isEqual(beta,1.0) == BT_FALSE )
1648  for (k = 0; k < xN; k++)
1649  for (j = 0; j < nRows; j++)
1650  y[j+k*yLD] *= beta;
1651 
1652  if ( isEqual(alpha,1.0) == BT_TRUE )
1653  for (k = 0; k < xN; k++)
1654  for (j = 0; j < nRows; j++)
1655  for (i = jr[j]; i < jr[j+1]; i++)
1656  y[j+k*yLD] += val[i] * x[ic[i]+k*xLD];
1657  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1658  for (k = 0; k < xN; k++)
1659  for (j = 0; j < nRows; j++)
1660  for (i = jr[j]; i < jr[j+1]; i++)
1661  y[j+k*yLD] -= val[i] * x[ic[i]+k*xLD];
1662  else
1663  for (k = 0; k < xN; k++)
1664  for (j = 0; j < nRows; j++)
1665  for (i = jr[j]; i < jr[j+1]; i++)
1666  y[j+k*yLD] += alpha * val[i] * x[ic[i]+k*xLD];
1667 
1668  return SUCCESSFUL_RETURN;
1669 }
1670 
1671 
1672 returnValue SparseMatrixRow::transTimes(int_t xN, real_t alpha, const real_t *x, int_t xLD,
1673  real_t beta, real_t *y, int_t yLD) const
1674 {
1675  long i, j, k;
1676 
1677  if ( isZero(beta) == BT_TRUE )
1678  for (k = 0; k < xN; k++)
1679  for (j = 0; j < nCols; j++)
1680  y[j+k*yLD] = 0.0;
1681  else if ( isEqual(beta,-1.0) == BT_TRUE )
1682  for (k = 0; k < xN; k++)
1683  for (j = 0; j < nCols; j++)
1684  y[j+k*yLD] = -y[j+k*yLD];
1685  else if ( isEqual(beta,1.0) == BT_FALSE )
1686  for (k = 0; k < xN; k++)
1687  for (j = 0; j < nCols; j++)
1688  y[j+k*yLD] *= beta;
1689 
1690  if ( isEqual(alpha,1.0) == BT_TRUE )
1691  for (k = 0; k < xN; k++)
1692  for (i = 0; i < nRows; i++)
1693  for (j = jr[i]; j < jr[i+1]; j++)
1694  y[ic[j]+k*yLD] += val[j] * x[i+k*xLD];
1695  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1696  for (k = 0; k < xN; k++)
1697  for (i = 0; i < nRows; i++)
1698  for (j = jr[i]; j < jr[i+1]; j++)
1699  y[ic[j]+k*yLD] -= val[j] * x[i+k*xLD];
1700  else
1701  for (k = 0; k < xN; k++)
1702  for (i = 0; i < nRows; i++)
1703  for (j = jr[i]; j < jr[i+1]; j++)
1704  y[ic[j]+k*yLD] += alpha * val[j] * x[i+k*xLD];
1705 
1706  return SUCCESSFUL_RETURN;
1707 }
1708 
1709 
1710 returnValue SparseMatrixRow::times(const Indexlist* const irows, const Indexlist* const icols,
1711  int_t xN, real_t alpha, const real_t *x, int_t xLD, real_t beta, real_t *y, int_t yLD,
1712  BooleanType yCompr) const
1713 {
1714  long i, j, k, l, srt, row;
1715 
1716  if (yCompr == BT_TRUE)
1717  {
1718  if ( isZero(beta) == BT_TRUE )
1719  for (k = 0; k < xN; k++)
1720  for (j = 0; j < irows->length; j++)
1721  y[j+k*yLD] = 0.0;
1722  else if ( isEqual(beta,-1.0) == BT_TRUE )
1723  for (k = 0; k < xN; k++)
1724  for (j = 0; j < irows->length; j++)
1725  y[j+k*yLD] = -y[j+k*yLD];
1726  else if ( isEqual(beta,1.0) == BT_FALSE )
1727  for (k = 0; k < xN; k++)
1728  for (j = 0; j < irows->length; j++)
1729  y[j+k*yLD] *= beta;
1730 
1731  if (icols == 0)
1732  if ( isEqual(alpha,1.0) == BT_TRUE )
1733  for (l = 0; l < irows->length; l++)
1734  {
1735  srt = irows->iSort[l];
1736  row = irows->number[srt];
1737  for (j = jr[row]; j < jr[row+1]; j++)
1738  for (k = 0; k < xN; k++)
1739  y[k*yLD+srt] += val[j] * x[k*xLD+ic[j]];
1740  }
1741  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1742  for (l = 0; l < irows->length; l++)
1743  {
1744  srt = irows->iSort[l];
1745  row = irows->number[srt];
1746  for (j = jr[row]; j < jr[row+1]; j++)
1747  for (k = 0; k < xN; k++)
1748  y[k*yLD+srt] -= val[j] * x[k*xLD+ic[j]];
1749  }
1750  else
1751  for (l = 0; l < irows->length; l++)
1752  {
1753  srt = irows->iSort[l];
1754  row = irows->number[srt];
1755  for (j = jr[row]; j < jr[row+1]; j++)
1756  for (k = 0; k < xN; k++)
1757  y[k*yLD+srt] += alpha * val[j] * x[k*xLD+ic[j]];
1758  }
1759  else /* icols != 0 */
1760  if ( isEqual(alpha,1.0) == BT_TRUE )
1761  for (l = 0; l < irows->length; l++)
1762  {
1763  srt = irows->iSort[l];
1764  row = irows->number[srt];
1765  j = jr[row];
1766  i = 0;
1767  while (j < jr[row+1] && i < icols->length)
1768  if (ic[j] == icols->number[icols->iSort[i]])
1769  {
1770  for (k = 0; k < xN; k++)
1771  y[k*yLD+srt] += val[j] * x[k*xLD+icols->iSort[i]];
1772  j++, i++;
1773  }
1774  else if (ic[j] > icols->number[icols->iSort[i]]) i++;
1775  else j++;
1776  }
1777  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1778  for (l = 0; l < irows->length; l++)
1779  {
1780  srt = irows->iSort[l];
1781  row = irows->number[srt];
1782  j = jr[row];
1783  i = 0;
1784  while (j < jr[row+1] && i < icols->length)
1785  if (ic[j] == icols->number[icols->iSort[i]])
1786  {
1787  for (k = 0; k < xN; k++)
1788  y[k*yLD+srt] -= val[j] * x[k*xLD+icols->iSort[i]];
1789  j++, i++;
1790  }
1791  else if (ic[j] > icols->number[icols->iSort[i]]) i++;
1792  else j++;
1793  }
1794  else
1795  for (l = 0; l < irows->length; l++)
1796  {
1797  srt = irows->iSort[l];
1798  row = irows->number[srt];
1799  j = jr[row];
1800  i = 0;
1801  while (j < jr[row+1] && i < icols->length)
1802  if (ic[j] == icols->number[icols->iSort[i]])
1803  {
1804  for (k = 0; k < xN; k++)
1805  y[k*yLD+srt] += alpha * val[j] * x[k*xLD+icols->iSort[i]];
1806  j++, i++;
1807  }
1808  else if (ic[j] > icols->number[icols->iSort[i]]) i++;
1809  else j++;
1810  }
1811  }
1812  else /* y not compressed */
1813  {
1814  if ( isZero(beta) == BT_TRUE )
1815  for (k = 0; k < xN; k++)
1816  for (j = 0; j < irows->length; j++)
1817  y[irows->number[j]+k*yLD] = 0.0;
1818  else if ( isEqual(beta,-1.0) == BT_TRUE )
1819  for (k = 0; k < xN; k++)
1820  for (j = 0; j < irows->length; j++)
1821  y[irows->number[j]+k*yLD] = -y[j+k*yLD];
1822  else if ( isEqual(beta,1.0) == BT_FALSE )
1823  for (k = 0; k < xN; k++)
1824  for (j = 0; j < irows->length; j++)
1825  y[irows->number[j]+k*yLD] *= beta;
1826 
1827  if (icols == 0)
1828  if ( isEqual(alpha,1.0) == BT_TRUE )
1829  for (l = 0; l < irows->length; l++)
1830  {
1831  row = irows->number[irows->iSort[l]];
1832  for (j = jr[row]; j < jr[row+1]; j++)
1833  for (k = 0; k < xN; k++)
1834  y[k*yLD+row] += val[j] * x[k*xLD+ic[j]];
1835  }
1836  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1837  for (l = 0; l < irows->length; l++)
1838  {
1839  row = irows->number[irows->iSort[l]];
1840  for (j = jr[row]; j < jr[row+1]; j++)
1841  for (k = 0; k < xN; k++)
1842  y[k*yLD+row] -= val[j] * x[k*xLD+ic[j]];
1843  }
1844  else
1845  for (l = 0; l < irows->length; l++)
1846  {
1847  row = irows->number[irows->iSort[l]];
1848  for (j = jr[row]; j < jr[row+1]; j++)
1849  for (k = 0; k < xN; k++)
1850  y[k*yLD+row] += alpha * val[j] * x[k*xLD+ic[j]];
1851  }
1852  else /* icols != 0 */
1853  if ( isEqual(alpha,1.0) == BT_TRUE )
1854  for (l = 0; l < irows->length; l++)
1855  {
1856  row = irows->iSort[l];
1857  j = jr[irows->number[row]];
1858  i = 0;
1859  while (j < jr[irows->number[row]+1] && i < icols->length)
1860  if (ic[j] == icols->number[icols->iSort[i]])
1861  {
1862  for (k = 0; k < xN; k++)
1863  y[k*yLD+row] += val[j] * x[k*xLD+icols->iSort[i]];
1864  j++, i++;
1865  }
1866  else if (ic[j] > icols->number[icols->iSort[i]]) i++;
1867  else j++;
1868  }
1869  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1870  for (l = 0; l < irows->length; l++)
1871  {
1872  row = irows->iSort[l];
1873  j = jr[irows->number[row]];
1874  i = 0;
1875  while (j < jr[irows->number[row]+1] && i < icols->length)
1876  if (ic[j] == icols->number[icols->iSort[i]])
1877  {
1878  for (k = 0; k < xN; k++)
1879  y[k*yLD+row] -= val[j] * x[k*xLD+icols->iSort[i]];
1880  j++, i++;
1881  }
1882  else if (ic[j] > icols->number[icols->iSort[i]]) i++;
1883  else j++;
1884  }
1885  else
1886  for (l = 0; l < irows->length; l++)
1887  {
1888  row = irows->iSort[l];
1889  j = jr[irows->number[row]];
1890  i = 0;
1891  while (j < jr[irows->number[row]+1] && i < icols->length)
1892  if (ic[j] == icols->number[icols->iSort[i]])
1893  {
1894  for (k = 0; k < xN; k++)
1895  y[k*yLD+row] += alpha * val[j] * x[k*xLD+icols->iSort[i]];
1896  j++, i++;
1897  }
1898  else if (ic[j] > icols->number[icols->iSort[i]]) i++;
1899  else j++;
1900  }
1901  }
1902  return SUCCESSFUL_RETURN;
1903 }
1904 
1905 
1906 returnValue SparseMatrixRow::transTimes(const Indexlist* const irows, const Indexlist* const icols,
1907  int_t xN, real_t alpha, const real_t *x, int_t xLD, real_t beta, real_t *y, int_t yLD) const
1908 {
1909  long i, j, k, l, row, srt;
1910 
1911  if ( isZero(beta) == BT_TRUE )
1912  for (k = 0; k < xN; k++)
1913  for (j = 0; j < icols->length; j++)
1914  y[j+k*yLD] = 0.0;
1915  else if ( isEqual(beta,-1.0) == BT_TRUE )
1916  for (k = 0; k < xN; k++)
1917  for (j = 0; j < icols->length; j++)
1918  y[j+k*yLD] = -y[j+k*yLD];
1919  else if ( isEqual(beta,1.0) == BT_FALSE )
1920  for (k = 0; k < xN; k++)
1921  for (j = 0; j < icols->length; j++)
1922  y[j+k*yLD] *= beta;
1923 
1924  if ( isEqual(alpha,1.0) == BT_TRUE )
1925  for (l = 0; l < irows->length; l++)
1926  {
1927  srt = irows->iSort[l];
1928  row = irows->number[srt];
1929  j = jr[row];
1930  i = 0;
1931  while (j < jr[row+1] && i < icols->length)
1932  if (ic[j] == icols->number[icols->iSort[i]])
1933  {
1934  for (k = 0; k < xN; k++)
1935  y[k*yLD+icols->iSort[i]] += val[j] * x[k*xLD+srt];
1936  j++, i++;
1937  }
1938  else if (ic[j] > icols->number[icols->iSort[i]]) i++;
1939  else j++;
1940  }
1941  else if ( isEqual(alpha,-1.0) == BT_TRUE )
1942  for (l = 0; l < irows->length; l++)
1943  {
1944  srt = irows->iSort[l];
1945  row = irows->number[srt];
1946  j = jr[row];
1947  i = 0;
1948  while (j < jr[row+1] && i < icols->length)
1949  if (ic[j] == icols->number[icols->iSort[i]])
1950  {
1951  for (k = 0; k < xN; k++)
1952  y[k*yLD+icols->iSort[i]] -= val[j] * x[k*xLD+srt];
1953  j++, i++;
1954  }
1955  else if (ic[j] > icols->number[icols->iSort[i]]) i++;
1956  else j++;
1957  }
1958  else
1959  for (l = 0; l < irows->length; l++)
1960  {
1961  srt = irows->iSort[l];
1962  row = irows->number[srt];
1963  j = jr[row];
1964  i = 0;
1965  while (j < jr[row+1] && i < icols->length)
1966  if (ic[j] == icols->number[icols->iSort[i]])
1967  {
1968  for (k = 0; k < xN; k++)
1969  y[k*yLD+icols->iSort[i]] += alpha * val[j] * x[k*xLD+srt];
1970  j++, i++;
1971  }
1972  else if (ic[j] > icols->number[icols->iSort[i]]) i++;
1973  else j++;
1974  }
1975 
1976  return SUCCESSFUL_RETURN;
1977 }
1978 
1979 
1980 
1981 returnValue SparseMatrixRow::addToDiag(real_t alpha)
1982 {
1983  long i;
1984 
1985  if ( jd == 0 )
1986  return THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
1987 
1988  if ( isZero(alpha) == BT_FALSE )
1989  {
1990  for (i = 0; i < nRows && i < nCols; i++)
1991  {
1992  if (ic[jd[i]] == i)
1993  val[jd[i]] += alpha;
1994  else
1995  return RET_NO_DIAGONAL_AVAILABLE;
1996  }
1997  }
1998 
1999  return SUCCESSFUL_RETURN;
2000 }
2001 
2002 
2003 sparse_int_t *SparseMatrixRow::createDiagInfo()
2004 {
2005  sparse_int_t i, j;
2006 
2007  if (jd == 0) {
2008  jd = new sparse_int_t[nRows];
2009 
2010  for (i = 0; i < nRows; i++)
2011  {
2012  for (j = jr[i]; j < jr[i+1] && ic[j] < i; j++);
2013  jd[i] = j;
2014  }
2015  }
2016 
2017  return jd;
2018 }
2019 
2020 
2021 real_t *SparseMatrixRow::full() const
2022 {
2023  sparse_int_t i, j;
2024  real_t *v = new real_t[nRows*nCols];
2025 
2026  for (i = 0; i < nCols*nRows; i++)
2027  v[i] = 0.0;
2028 
2029  for (i = 0; i < nRows; i++)
2030  for (j = jr[i]; j < jr[i+1]; j++)
2031  v[ic[j] + i * nCols] = val[j];
2032 
2033  return v;
2034 }
2035 
2036 
2037 returnValue SparseMatrixRow::print( const char* name ) const
2038 {
2039  real_t* tmp = this->full();
2040  returnValue retVal = REFER_NAMESPACE_QPOASES print( tmp,nRows,nCols,name );
2041  delete[] tmp;
2042 
2043  return retVal;
2044 }
2045 
2046 returnValue SparseMatrixRow::writeToFile( FILE* output_file, const char* prefix ) const
2047 {
2048  return THROWERROR( RET_NOT_YET_IMPLEMENTED );
2049 }
2050 
2051 Matrix *SymSparseMat::duplicate() const
2052 {
2053  return duplicateSym();
2054 }
2055 
2056 
2057 SymmetricMatrix *SymSparseMat::duplicateSym() const
2058 {
2059  /* "same" as duplicate() in SparseMatrix */
2060  long i, length = jc[nCols];
2061  SymSparseMat *dupl = new SymSparseMat;
2062 
2063  dupl->nRows = nRows;
2064  dupl->nCols = nCols;
2065  dupl->ir = new sparse_int_t[length];
2066  dupl->jc = new sparse_int_t[nCols+1];
2067  dupl->val = new real_t[length];
2068 
2069  for (i = 0; i < length; i++) dupl->ir[i] = ir[i];
2070  for (i = 0; i <= nCols; i++) dupl->jc[i] = jc[i];
2071  for (i = 0; i < length; i++) dupl->val[i] = val[i];
2072 
2073  if ( jd != 0 )
2074  {
2075  dupl->jd = new sparse_int_t[nCols];
2076  for (i = 0; i < nCols; i++) dupl->jd[i] = jd[i];
2077  }
2078  else
2079  dupl->jd = 0;
2080 
2081  dupl->doFreeMemory( );
2082 
2083  return dupl;
2084 }
2085 
2086 
2087 returnValue SymSparseMat::bilinear(const Indexlist* const icols,
2088  int_t xN, const real_t *x, int_t xLD, real_t *y, int_t yLD) const
2089 {
2090  int_t i, j, k, l, idx, row, col;
2091 
2092  if ( jd == 0 )
2093  return THROWERROR( RET_DIAGONAL_NOT_INITIALISED );
2094 
2095  /* clear output */
2096  for (i = 0; i < xN*xN; i++)
2097  y[i] = 0.0;
2098 
2099  /* compute lower triangle */
2100  for (l = 0; l < icols->length; l++)
2101  {
2102  col = icols->number[icols->iSort[l]];
2103  idx = jd[col];
2104  k = 0;
2105  while (idx < jc[col+1] && k < icols->length)
2106  {
2107  row = icols->number[icols->iSort[k]];
2108  if (ir[idx] == row)
2109  {
2110  /* TODO: It is possible to formulate this as DSYR and DSYR2
2111  * operations. */
2112  if (row == col) /* diagonal element */
2113  for (i = 0; i < xN; i++)
2114  for (j = i; j < xN; j++)
2115  y[i*yLD+j] += val[idx] * x[i*xLD+col] * x[j*xLD+col];
2116  else /* subdiagonal elements */
2117  for (i = 0; i < xN; i++)
2118  for (j = i; j < xN; j++)
2119  y[i*yLD+j] += val[idx] * (x[i*xLD+col] * x[j*xLD+row] + x[i*xLD+row] * x[j*xLD+col]);
2120  idx++, k++;
2121  }
2122  else if (ir[idx] > row) k++;
2123  else idx++;
2124  }
2125  }
2126 
2127  /* fill upper triangle */
2128  for (i = 0; i < xN; i++)
2129  for (j = i; j < xN; j++)
2130  y[j*yLD+i] = y[i*yLD+j];
2131 
2132  return SUCCESSFUL_RETURN;
2133 }
2134 
2135 
2136 END_NAMESPACE_QPOASES
2137 
2138 
2139 /*
2140  * end of file
2141  */
getMax
int getMax(int x, int y)
Matrix::getNorm
virtual real_t getNorm(int_t type=2) const =0
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
getSqrt
real_t getSqrt(real_t x)
Matrix::print
virtual returnValue print() const =0
SparseMatrixRow::getRowNorm
virtual real_t getRowNorm(int_t rNum, int_t type=2) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:1503
getAbs
real_t getAbs(real_t x)
isZero
BooleanType isZero(real_t x, real_t TOL=ZERO)
DenseMatrix::isDiag
virtual BooleanType isDiag() const
Definition: qpOASES-3.0beta/src/Matrices.cpp:89
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
DenseMatrix::addToDiag
virtual returnValue addToDiag(real_t alpha)
Definition: qpOASES-3.0beta/src/Matrices.cpp:406
DenseMatrix::getRow
virtual returnValue getRow(int rNum, const Indexlist *const icols, real_t alpha, real_t *row) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:104
INFTY
const double INFTY
Definition: acado_constants.hpp:65
SymDenseMat::duplicate
virtual Matrix * duplicate() const
Definition: qpOASES-3.0beta/src/Matrices.cpp:424
SymDenseMat::duplicateSym
virtual SymmetricMatrix * duplicateSym() const
Definition: qpOASES-3.2.0/src/Matrices.cpp:598
DenseMatrix::getCol
virtual returnValue getCol(int cNum, const Indexlist *const irows, real_t alpha, real_t *col) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:134
DenseMatrix::full
virtual real_t * full() const
Definition: qpOASES-3.2.0/src/Matrices.cpp:577
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
EPS
BEGIN_NAMESPACE_ACADO const double EPS
Definition: acado_constants.hpp:43
RET_NO_DIAGONAL_AVAILABLE
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:201
SparseMatrix::duplicate
virtual Matrix * duplicate() const
Definition: qpOASES-3.0beta/src/Matrices.cpp:533
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
DenseMatrix::getSparseSubmatrix
virtual returnValue getSparseSubmatrix(int_t irowsLength, const int_t *const irowsNumber, int_t icolsLength, const int_t *const icolsNumber, int_t rowoffset, int_t coloffset, int_t &numNonzeros, int_t *irn, int_t *jcn, real_t *avals, BooleanType only_lower_triangular=BT_FALSE) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:214
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
SymSparseMat::duplicateSym
virtual SymmetricMatrix * duplicateSym() const
Definition: qpOASES-3.2.0/src/Matrices.cpp:2057
THROWERROR
#define THROWERROR(retval)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:391
Matrix::getSparseSubmatrix
virtual returnValue getSparseSubmatrix(const Indexlist *const irows, const Indexlist *const icols, int_t rowoffset, int_t coloffset, int_t &numNonzeros, int_t *irn, int_t *jcn, real_t *avals, BooleanType only_lower_triangular=BT_FALSE) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:46
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
SparseMatrix::writeToFile
virtual returnValue writeToFile(FILE *output_file, const char *prefix) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:1358
SymSparseMat::duplicate
virtual Matrix * duplicate() const
Definition: qpOASES-3.0beta/src/Matrices.cpp:1085
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
DenseMatrix::getNorm
virtual real_t getNorm(int_t type=2) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:154
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
DenseMatrix::diag
virtual real_t diag(int i) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:83
DenseMatrix
Interfaces matrix-vector operations tailored to general dense matrices.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:273
BooleanType
BooleanType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:44
SparseMatrix
Interfaces matrix-vector operations tailored to general sparse matrices.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:443
SparseMatrix::print
virtual returnValue print() const
Definition: qpOASES-3.0beta/src/Matrices.cpp:1078
SparseMatrix::getNorm
virtual real_t getNorm(int_t type=2) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:780
SparseMatrix::getSparseSubmatrix
virtual returnValue getSparseSubmatrix(int_t irowsLength, const int_t *const irowsNumber, int_t icolsLength, const int_t *const icolsNumber, int_t rowoffset, int_t coloffset, int_t &numNonzeros, int_t *irn, int_t *jcn, real_t *avals, BooleanType only_lower_triangular=BT_FALSE) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:907
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
DenseMatrix::duplicate
virtual Matrix * duplicate() const
Definition: qpOASES-3.0beta/src/Matrices.cpp:64
DenseMatrix::print
virtual returnValue print() const
Definition: qpOASES-3.0beta/src/Matrices.cpp:416
SparseMatrix::addToDiag
virtual returnValue addToDiag(real_t alpha)
Definition: qpOASES-3.0beta/src/Matrices.cpp:1031
Indexlist::getLength
int getLength()
SparseMatrixRow::writeToFile
virtual returnValue writeToFile(FILE *output_file, const char *prefix) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:2046
Matrix::free
virtual void free()=0
Matrix
Abstract base class for interfacing tailored matrix-vector operations.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:112
SparseMatrix::getRowNorm
virtual real_t getRowNorm(int_t rNum, int_t type=2) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:788
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
SparseMatrix::diag
virtual real_t diag(int i) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:555
RET_DIAGONAL_NOT_INITIALISED
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:217
v
#define v
END_NAMESPACE_QPOASES
#define END_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:120
SparseMatrixRow::addToDiag
virtual returnValue addToDiag(real_t alpha)
Definition: qpOASES-3.2.0/src/Matrices.cpp:1981
uint_t
unsigned int uint_t
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:175
Matrix::needToFreeMemory
BooleanType needToFreeMemory() const
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:221
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
SparseMatrixRow::getSparseSubmatrix
virtual returnValue getSparseSubmatrix(int_t irowsLength, const int_t *const irowsNumber, int_t icolsLength, const int_t *const icolsNumber, int_t rowoffset, int_t coloffset, int_t &numNonzeros, int_t *irn, int_t *jcn, real_t *avals, BooleanType only_lower_triangular=BT_FALSE) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:1623
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
SparseMatrixRow::getNorm
virtual real_t getNorm(int_t type=2) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:1494
SparseMatrixRow::isDiag
virtual BooleanType isDiag() const
Definition: qpOASES-3.2.0/src/Matrices.cpp:1473
SparseMatrixRow::diag
virtual real_t diag(int_t i) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:1460
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
SparseMatrix::isDiag
virtual BooleanType isDiag() const
Definition: qpOASES-3.0beta/src/Matrices.cpp:561
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
RET_NOT_YET_IMPLEMENTED
Definition: acado_types.hpp:847
real_t
double real_t
Definition: AD_test.c:10
DenseMatrix::writeToFile
virtual returnValue writeToFile(FILE *output_file, const char *prefix) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:571
DenseMatrix::getRowNorm
virtual real_t getRowNorm(int_t rNum, int_t type=2) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:161
isEqual
BooleanType isEqual(real_t x, real_t y, real_t TOL=ZERO)
SparseMatrixRow::duplicate
virtual Matrix * duplicate() const
Definition: qpOASES-3.2.0/src/Matrices.cpp:1430
REFER_NAMESPACE_QPOASES
#define REFER_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:126
SparseMatrixRow::full
virtual real_t * full() const
Definition: qpOASES-3.2.0/src/Matrices.cpp:2021
SymmetricMatrix
Abstract base class for interfacing matrix-vector operations tailored to symmetric matrices...
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:246

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