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