Matrices.cpp
Go to the documentation of this file.
00001 /*
00002  *      This file is part of qpOASES.
00003  *
00004  *      qpOASES -- An Implementation of the Online Active Set Strategy.
00005  *      Copyright (C) 2007-2011 by Hans Joachim Ferreau, Andreas Potschka,
00006  *      Christian Kirches et al. All rights reserved.
00007  *
00008  *      qpOASES is free software; you can redistribute it and/or
00009  *      modify it under the terms of the GNU Lesser General Public
00010  *      License as published by the Free Software Foundation; either
00011  *      version 2.1 of the License, or (at your option) any later version.
00012  *
00013  *      qpOASES is distributed in the hope that it will be useful,
00014  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016  *      See the GNU Lesser General Public License for more details.
00017  *
00018  *      You should have received a copy of the GNU Lesser General Public
00019  *      License along with qpOASES; if not, write to the Free Software
00020  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00021  *
00022  */
00023 
00024 
00035 #include <qpOASES.hpp>
00036 
00037 #include <qpOASES/PrivateUtils.hpp>
00038 
00039 BEGIN_NAMESPACE_QPOASES
00040 
00042 const char * const TRANS = "TRANS";
00043 
00045 const char * const NOTRANS = "NOTRANS";
00046 
00047 /*****************************************************************************
00048  *  P U B L I C                                                              *
00049  *****************************************************************************/
00050 
00051 DenseMatrix::~DenseMatrix()
00052 {
00053         if ( needToFreeMemory( ) == BT_TRUE )
00054                 free( );
00055 }
00056 
00057 void DenseMatrix::free( )
00058 {
00059         if (val != 0)
00060                 delete[] val;
00061         val = 0;
00062 }
00063 
00064 Matrix *DenseMatrix::duplicate( ) const
00065 {
00066         DenseMatrix *dupl = 0;
00067 
00068         if ( needToFreeMemory( ) == BT_TRUE )
00069         {
00070                 real_t* val_new = new real_t[nRows*nCols];
00071                 memcpy( val_new,val, nRows*nCols*sizeof(real_t) );
00072                 dupl = new DenseMatrix(nRows, nCols, nCols, val_new);
00073                 dupl->doFreeMemory( );
00074         }
00075         else
00076         {
00077                 dupl = new DenseMatrix(nRows, nCols, nCols, val);
00078         }
00079 
00080         return dupl;
00081 }
00082 
00083 real_t DenseMatrix::diag(       int i
00084                                                         ) const
00085 {
00086         return val[i*(leaDim+1)];
00087 }
00088 
00089 BooleanType DenseMatrix::isDiag( ) const
00090 {
00091         int i, j;
00092 
00093         if (nRows != nCols)
00094                 return BT_FALSE;
00095 
00096         for ( i=0; i<nRows; ++i )
00097                 for ( j=0; j<i; ++j )
00098                         if ( ( fabs( val[i*leaDim+j] ) > EPS ) || ( fabs( val[j*leaDim+i] ) > EPS ) )
00099                                 return BT_FALSE;
00100 
00101         return BT_TRUE;
00102 }
00103 
00104 returnValue DenseMatrix::getRow(int rNum, const Indexlist* const icols, real_t alpha, real_t *row) const
00105 {
00106         int i;
00107     if (icols != 0)
00108     {
00109             if (isExactlyOne(alpha))
00110                     for (i = 0; i < icols->length; i++)
00111                             row[i] = val[rNum*leaDim+icols->number[i]];
00112             else if (isExactlyMinusOne(alpha))
00113                     for (i = 0; i < icols->length; i++)
00114                             row[i] = -val[rNum*leaDim+icols->number[i]];
00115             else
00116                     for (i = 0; i < icols->length; i++)
00117                             row[i] = alpha*val[rNum*leaDim+icols->number[i]];
00118     }
00119     else
00120     {
00121             if (isExactlyOne(alpha))
00122                     for (i = 0; i < nCols; i++)
00123                             row[i] = val[rNum*leaDim+i];
00124             else if (isExactlyMinusOne(alpha))
00125                     for (i = 0; i < nCols; i++)
00126                             row[i] = -val[rNum*leaDim+i];
00127             else
00128                     for (i = 0; i < nCols; i++)
00129                             row[i] = alpha*val[rNum*leaDim+i];
00130     }
00131         return SUCCESSFUL_RETURN;
00132 }
00133 
00134 returnValue DenseMatrix::getCol(int cNum, const Indexlist* const irows, real_t alpha, real_t *col) const
00135 {
00136         int i;
00137 
00138         if (isExactlyOne(alpha))
00139                 for (i = 0; i < irows->length; i++)
00140                         col[i] = val[irows->number[i]*leaDim+cNum];
00141         else if (isExactlyMinusOne(alpha))
00142                 for (i = 0; i < irows->length; i++)
00143                         col[i] = -val[irows->number[i]*leaDim+cNum];
00144         else
00145                 for (i = 0; i < irows->length; i++)
00146                         col[i] = alpha*val[irows->number[i]*leaDim+cNum];
00147 
00148         return SUCCESSFUL_RETURN;
00149 }
00150 
00151 returnValue DenseMatrix::times(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
00152 {
00153         unsigned long _xN=xN, _nRows=nRows, _nCols=nCols, /*_leaDim=getMax(1,leaDim),*/ _xLD=getMax(1,xLD), _yLD=getMax(1,yLD);
00154         /* Call BLAS. Mind row major format! */
00155         GEMM(TRANS, NOTRANS, &_nRows, &_xN, &_nCols, &alpha, val, &_nCols, x, &_xLD, &beta, y, &_yLD);
00156         return SUCCESSFUL_RETURN;
00157 }
00158 
00159 returnValue DenseMatrix::transTimes(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
00160 {
00161         /* Call BLAS. Mind row major format! */
00162         unsigned long _xN=xN, _nRows=nRows, _nCols=nCols, /*_leaDim=getMax(1,leaDim),*/ _xLD=getMax(1,xLD), _yLD=getMax(1,yLD);
00163         GEMM(NOTRANS, NOTRANS, &_nCols, &_xN, &_nRows, &alpha, val, &_nCols, x, &_xLD, &beta, y, &_yLD);
00164         return SUCCESSFUL_RETURN;
00165 }
00166 
00167 returnValue DenseMatrix::times(const Indexlist* const irows, const Indexlist* const icols,
00168                 int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD,
00169                 BooleanType yCompr) const
00170 {
00171         int i, j, k, row, col, iy, irA;
00172 
00173         if (yCompr == BT_TRUE)
00174         {
00175                 if (isExactlyZero(beta))
00176                         for (k = 0; k < xN; k++)
00177                                 for (j = 0; j < irows->length; j++)
00178                                         y[j+k*yLD] = 0.0;
00179                 else if (isExactlyMinusOne(beta))
00180                         for (k = 0; k < xN; k++)
00181                                 for (j = 0; j < irows->length; j++)
00182                                         y[j+k*yLD] = -y[j+k*yLD];
00183                 else if (!isExactlyOne(beta))
00184                         for (k = 0; k < xN; k++)
00185                                 for (j = 0; j < irows->length; j++)
00186                                         y[j+k*yLD] *= beta;
00187 
00188                 if (icols == 0)
00189                         if (isExactlyOne(alpha))
00190                                 for (k = 0; k < xN; k++)
00191                                         for (j = 0; j < irows->length; j++)
00192                                         {
00193                                                 row = irows->iSort[j];
00194                                                 iy = row + k * yLD;
00195                                                 irA = irows->number[row] * leaDim;
00196                                                 for (i = 0; i < nCols; i++)
00197                                                         y[iy] += val[irA+i] * x[k*xLD+i];
00198                                         }
00199                         else if (isExactlyMinusOne(alpha))
00200                                 for (k = 0; k < xN; k++)
00201                                         for (j = 0; j < irows->length; j++)
00202                                         {
00203                                                 row = irows->iSort[j];
00204                                                 iy = row + k * yLD;
00205                                                 irA = irows->number[row] * leaDim;
00206                                                 for (i = 0; i < nCols; i++)
00207                                                         y[iy] -= val[irA+i] * x[k*xLD+i];
00208                                         }
00209                         else
00210                                 for (k = 0; k < xN; k++)
00211                                         for (j = 0; j < irows->length; j++)
00212                                         {
00213                                                 row = irows->iSort[j];
00214                                                 iy = row + k * yLD;
00215                                                 irA = irows->number[row] * leaDim;
00216                                                 for (i = 0; i < nCols; i++)
00217                                                         y[iy] += alpha * val[irA+i] * x[k*xLD+i];
00218                                         }
00219                 else /* icols != 0 */
00220                         if (isExactlyOne(alpha))
00221                                 for (k = 0; k < xN; k++)
00222                                         for (j = 0; j < irows->length; j++)
00223                                         {
00224                                                 row = irows->iSort[j];
00225                                                 iy = row + k * yLD;
00226                                                 irA = irows->number[row] * leaDim;
00227                                                 for (i = 0; i < icols->length; i++)
00228                                                 {
00229                                                         col = icols->iSort[i];
00230                                                         y[iy] += val[irA+icols->number[col]] * x[k*xLD+col];
00231                                                 }
00232                                         }
00233                         else if (isExactlyMinusOne(alpha))
00234                                 for (k = 0; k < xN; k++)
00235                                         for (j = 0; j < irows->length; j++)
00236                                         {
00237                                                 row = irows->iSort[j];
00238                                                 iy = row + k * yLD;
00239                                                 irA = irows->number[row] * leaDim;
00240                                                 for (i = 0; i < icols->length; i++)
00241                                                 {
00242                                                         col = icols->iSort[i];
00243                                                         y[iy] -= val[irA+icols->number[col]] * x[k*xLD+col];
00244                                                 }
00245                                         }
00246                         else
00247                                 for (k = 0; k < xN; k++)
00248                                         for (j = 0; j < irows->length; j++)
00249                                         {
00250                                                 row = irows->iSort[j];
00251                                                 iy = row + k * yLD;
00252                                                 irA = irows->number[row] * leaDim;
00253                                                 for (i = 0; i < icols->length; i++)
00254                                                 {
00255                                                         col = icols->iSort[i];
00256                                                         y[iy] += alpha * val[irA+icols->number[col]] * x[k*xLD+col];
00257                                                 }
00258                                         }
00259         }
00260         else /* y not compressed */
00261         {
00262                 if (isExactlyZero(beta))
00263                         for (k = 0; k < xN; k++)
00264                                 for (j = 0; j < irows->length; j++)
00265                                         y[irows->number[j]+k*yLD] = 0.0;
00266                 else if (isExactlyMinusOne(beta))
00267                         for (k = 0; k < xN; k++)
00268                                 for (j = 0; j < irows->length; j++)
00269                                         y[irows->number[j]+k*yLD] = -y[j+k*yLD];
00270                 else if (!isExactlyOne(beta))
00271                         for (k = 0; k < xN; k++)
00272                                 for (j = 0; j < irows->length; j++)
00273                                         y[irows->number[j]+k*yLD] *= beta;
00274 
00275                 if (icols == 0)
00276                         if (isExactlyOne(alpha))
00277                                 for (k = 0; k < xN; k++)
00278                                         for (j = 0; j < irows->length; j++)
00279                                         {
00280                                                 row = irows->number[irows->iSort[j]];
00281                                                 iy = row + k * yLD;
00282                                                 irA = row * leaDim;
00283                                                 for (i = 0; i < nCols; i++)
00284                                                         y[iy] += val[irA+i] * x[k*xLD+i];
00285                                         }
00286                         else if (isExactlyMinusOne(alpha))
00287                                 for (k = 0; k < xN; k++)
00288                                         for (j = 0; j < irows->length; j++)
00289                                         {
00290                                                 row = irows->number[irows->iSort[j]];
00291                                                 iy = row + k * yLD;
00292                                                 irA = row * leaDim;
00293                                                 for (i = 0; i < nCols; i++)
00294                                                         y[iy] -= val[irA+i] * x[k*xLD+i];
00295                                         }
00296                         else
00297                                 for (k = 0; k < xN; k++)
00298                                         for (j = 0; j < irows->length; j++)
00299                                         {
00300                                                 row = irows->number[irows->iSort[j]];
00301                                                 iy = row + k * yLD;
00302                                                 irA = row * leaDim;
00303                                                 for (i = 0; i < nCols; i++)
00304                                                         y[iy] += alpha * val[irA+i] * x[k*xLD+i];
00305                                         }
00306                 else /* icols != 0 */
00307                         if (isExactlyOne(alpha))
00308                                 for (k = 0; k < xN; k++)
00309                                         for (j = 0; j < irows->length; j++)
00310                                         {
00311                                                 row = irows->number[irows->iSort[j]];
00312                                                 iy = row + k * yLD;
00313                                                 irA = row * leaDim;
00314                                                 for (i = 0; i < icols->length; i++)
00315                                                 {
00316                                                         col = icols->iSort[i];
00317                                                         y[iy] += val[irA+icols->number[col]] * x[k*xLD+col];
00318                                                 }
00319                                         }
00320                         else if (isExactlyMinusOne(alpha))
00321                                 for (k = 0; k < xN; k++)
00322                                         for (j = 0; j < irows->length; j++)
00323                                         {
00324                                                 row = irows->number[irows->iSort[j]];
00325                                                 iy = row + k * yLD;
00326                                                 irA = row * leaDim;
00327                                                 for (i = 0; i < icols->length; i++)
00328                                                 {
00329                                                         col = icols->iSort[i];
00330                                                         y[iy] -= val[irA+icols->number[col]] * x[k*xLD+col];
00331                                                 }
00332                                         }
00333                         else
00334                                 for (k = 0; k < xN; k++)
00335                                         for (j = 0; j < irows->length; j++)
00336                                         {
00337                                                 row = irows->number[irows->iSort[j]];
00338                                                 iy = row + k * yLD;
00339                                                 irA = row * leaDim;
00340                                                 for (i = 0; i < icols->length; i++)
00341                                                 {
00342                                                         col = icols->iSort[i];
00343                                                         y[iy] += alpha * val[irA+icols->number[col]] * x[k*xLD+col];
00344                                                 }
00345                                         }
00346         }
00347 
00348         return SUCCESSFUL_RETURN;
00349 }
00350 
00351 returnValue DenseMatrix::transTimes(const Indexlist* const irows, const Indexlist* const icols,
00352                 int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
00353 {
00354         int i, j, k, row, col;
00355 
00356         if (isExactlyZero(beta))
00357                 for (k = 0; k < xN; k++)
00358                         for (j = 0; j < icols->length; j++)
00359                                 y[j+k*yLD] = 0.0;
00360         else if (isExactlyMinusOne(beta))
00361                 for (k = 0; k < xN; k++)
00362                         for (j = 0; j < icols->length; j++)
00363                                 y[j+k*yLD] = -y[j+k*yLD];
00364         else if (!isExactlyOne(beta))
00365                 for (k = 0; k < xN; k++)
00366                         for (j = 0; j < icols->length; j++)
00367                                 y[j+k*yLD] *= beta;
00368 
00369         if (isExactlyOne(alpha))
00370                 for (k = 0; k < xN; k++)
00371                         for (j = 0; j < irows->length; j++)
00372                         {
00373                                 row = irows->iSort[j];
00374                                 for (i = 0; i < icols->length; i++)
00375                                 {
00376                                         col = icols->iSort[i];
00377                                         y[col+k*yLD] += val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
00378                                 }
00379                         }
00380         else if (isExactlyMinusOne(alpha))
00381                 for (k = 0; k < xN; k++)
00382                         for (j = 0; j < irows->length; j++)
00383                         {
00384                                 row = irows->iSort[j];
00385                                 for (i = 0; i < icols->length; i++)
00386                                 {
00387                                         col = icols->iSort[i];
00388                                         y[col+k*yLD] -= val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
00389                                 }
00390                         }
00391         else
00392                 for (k = 0; k < xN; k++)
00393                         for (j = 0; j < irows->length; j++)
00394                         {
00395                                 row = irows->iSort[j];
00396                                 for (i = 0; i < icols->length; i++)
00397                                 {
00398                                         col = icols->iSort[i];
00399                                         y[col+k*yLD] += alpha * val[irows->number[row]*leaDim+icols->number[col]] * x[row+k*xLD];
00400                                 }
00401                         }
00402 
00403         return SUCCESSFUL_RETURN;
00404 }
00405 
00406 returnValue DenseMatrix::addToDiag(real_t alpha)
00407 {
00408         int i;
00409         for (i = 0; i < nRows && i < nCols; i++)
00410                 val[i*(leaDim+1)] += alpha;
00411 
00412         return SUCCESSFUL_RETURN;
00413 }
00414 
00415 
00416 returnValue DenseMatrix::print( ) const
00417 {
00418         myPrintf( "aaa\n" );
00419         return qpOASES::print( val,nRows,nCols );
00420 }
00421 
00422 
00423 
00424 Matrix *SymDenseMat::duplicate( ) const
00425 {
00426         /* "same" as duplicate() in DenseMatrix */
00427         SymDenseMat *dupl = 0;
00428 
00429         if ( needToFreeMemory( ) == BT_TRUE )
00430         {
00431                 real_t* val_new = new real_t[nRows*nCols];
00432                 memcpy( val_new,val, nRows*nCols*sizeof(real_t) );
00433                 dupl = new SymDenseMat(nRows, nCols, nCols, val_new);
00434                 dupl->doFreeMemory( );
00435         }
00436         else
00437         {
00438                 dupl = new SymDenseMat(nRows, nCols, nCols, val);
00439         }
00440 
00441         return dupl;
00442 }
00443 
00444 
00445 returnValue SymDenseMat::bilinear(const Indexlist* const icols,
00446                 int xN, const real_t *x, int xLD, real_t *y, int yLD) const
00447 {
00448         int ii, jj, kk, col;
00449         int i,j,k,irA;
00450 
00451         for (ii = 0; ii < xN; ii++)
00452                 for (jj = 0; jj < xN; jj++)
00453                         y[ii*yLD+jj] = 0.0;
00454                         
00455         real_t *Ax = new real_t[icols->length * xN];                    
00456 
00457         for (i=0;i<icols->length * xN;++i)
00458                 Ax[i]=0.0;
00459 
00460         /* exploit symmetry of A ! */
00461         for (j = 0; j < icols->length; j++) {
00462                 irA = icols->number[j] * leaDim;        
00463                 for (i = 0; i < icols->length; i++)
00464                 {
00465                         real_t h = val[irA+icols->number[i]];
00466                         for (k = 0; k < xN; k++)
00467                                 Ax[j + k * icols->length] += h * x[k*xLD+icols->number[i]];
00468                 }
00469         }
00470                                                         
00471         for (ii = 0; ii < icols->length; ++ii) {
00472                 col = icols->number[ii];
00473                 for (jj = 0; jj < xN; ++jj) {
00474                         for (kk = 0; kk < xN; ++kk) {
00475                                 y[kk + jj*yLD] += x[col + jj*xLD] * Ax[ii + kk*icols->length];
00476                         }
00477                 }
00478         }
00479         delete[] Ax;
00480 
00481         return SUCCESSFUL_RETURN;
00482 }
00483 
00484 
00485 SparseMatrix::SparseMatrix() : nRows(0), nCols(0), ir(0), jc(0), jd(0), val(0) {}
00486 
00487 SparseMatrix::SparseMatrix(long nr, long nc, long *r, long *c, real_t *v, long *d)
00488         : nRows(nr), nCols(nc), ir(r), jc(c), jd(d), val(v) { doNotFreeMemory(); }
00489 
00490 SparseMatrix::SparseMatrix(int nr, int nc, int ld, const real_t * const v) : nRows(nr), nCols(nc), jd(0)
00491 {
00492         int i, j, nnz;
00493 
00494         jc = new long[nc+1];
00495         ir = new long[nr*nc];
00496         val = new real_t[nr*nc];
00497 
00498         nnz = 0;
00499         for (j = 0; j < nCols; j++)
00500         {
00501                 jc[j] = nnz;
00502                 for (i = 0; i < nRows; i++)
00503                         if (!isExactlyZero(v[i*ld+j]))
00504                         {
00505                                 ir[nnz] = i;
00506                                 val[nnz++] = v[i*ld+j];
00507                         }
00508         }
00509         jc[nCols] = nnz;
00510 
00511         doFreeMemory( );
00512 }
00513 
00514 SparseMatrix::~SparseMatrix()
00515 {
00516         if ( needToFreeMemory() == BT_TRUE ) 
00517                 free( );
00518 }
00519 
00520 void SparseMatrix::free( )
00521 {
00522         if (ir != 0) delete[] ir;
00523         ir = 0;
00524         if (jc != 0) delete[] jc;
00525         jc = 0;
00526         if (jd != 0) delete[] jd;
00527         jd = 0;
00528         if (val != 0) delete[] val;
00529         val = 0;
00530         doNotFreeMemory( );
00531 }
00532 
00533 Matrix *SparseMatrix::duplicate() const
00534 {
00535         long i, length = jc[nCols];
00536         SparseMatrix *dupl = new SparseMatrix;
00537 
00538         dupl->nRows = nRows;
00539         dupl->nCols = nCols;
00540         dupl->ir = new long[length];
00541         dupl->jc = new long[nCols+1];
00542         dupl->jd = new long[nCols];
00543         dupl->val = new real_t[length];
00544 
00545         for (i = 0; i < length; i++) dupl->ir[i] = ir[i];
00546         for (i = 0; i <= nCols; i++) dupl->jc[i] = jc[i];
00547         for (i = 0; i < nCols; i++) dupl->jd[i] = jd[i];
00548         for (i = 0; i < length; i++) dupl->val[i] = val[i];
00549         
00550         dupl->doFreeMemory( );
00551 
00552         return dupl;
00553 }
00554 
00555 real_t SparseMatrix::diag(int i) const
00556 {
00557         int entry = jd[i];
00558         return (entry < jc[i+1] && ir[entry] == i) ? val[entry] : 0.0;
00559 }
00560 
00561 BooleanType SparseMatrix::isDiag() const
00562 {
00563         return BT_FALSE;
00564 }
00565 
00566 returnValue SparseMatrix::getRow(int rNum, const Indexlist* const icols, real_t alpha, real_t *row) const
00567 {
00568         long i, j, k;
00569 
00570     if (icols != 0)
00571     {
00572             if (isExactlyOne(alpha))
00573                     for (k = 0; k < icols->length; k++)
00574                     {
00575                             j = icols->number[icols->iSort[k]];
00576                             for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
00577                             row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? val[i] : 0.0;
00578                     }
00579             else if (isExactlyMinusOne(alpha))
00580                     for (k = 0; k < icols->length; k++)
00581                     {
00582                             j = icols->number[icols->iSort[k]];
00583                             for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
00584                             row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? -val[i] : 0.0;
00585                     }
00586             else
00587                     for (k = 0; k < icols->length; k++)
00588                     {
00589                             j = icols->number[icols->iSort[k]];
00590                             for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
00591                             row[icols->iSort[k]] = (i < jc[j+1] && ir[i] == rNum) ? alpha*val[i] : 0.0;
00592                     }
00593     }
00594     else
00595     {
00596             if (isExactlyOne(alpha))
00597                     for (j = 0; j < nCols; j++)
00598                     {
00599                             for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
00600                             row[j] = (i < jc[j+1] && ir[i] == rNum) ? val[i] : 0.0;
00601                     }
00602             else if (isExactlyMinusOne(alpha))
00603                     for (j = 0; j < icols->length; j++)
00604                     {
00605                             for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
00606                             row[j] = (i < jc[j+1] && ir[i] == rNum) ? -val[i] : 0.0;
00607                     }
00608             else
00609                     for (j = 0; j < icols->length; j++)
00610                     {
00611                             for (i = jc[j]; i < jc[j+1] && ir[i] < rNum; i++);
00612                             row[j] = (i < jc[j+1] && ir[i] == rNum) ? alpha*val[i] : 0.0;
00613                     }
00614     }
00615         return SUCCESSFUL_RETURN;
00616 }
00617 
00618 returnValue SparseMatrix::getCol(int cNum, const Indexlist* const irows, real_t alpha, real_t *col) const
00619 {
00620         long i, j;
00621 
00622         i = jc[cNum];
00623         j = 0;
00624         if (isExactlyOne(alpha))
00625                 while (i < jc[cNum+1] && j < irows->length)
00626                         if (ir[i] == irows->number[irows->iSort[j]])
00627                                 col[irows->iSort[j++]] = val[i++];
00628                         else if (ir[i] > irows->number[irows->iSort[j]])
00629                                 col[irows->iSort[j++]] = 0.0;
00630                         else
00631                                 i++;
00632         else if (isExactlyMinusOne(alpha))
00633                 while (i < jc[cNum+1] && j < irows->length)
00634                         if (ir[i] == irows->number[irows->iSort[j]])
00635                                 col[irows->iSort[j++]] = -val[i++];
00636                         else if (ir[i] > irows->number[irows->iSort[j]])
00637                                 col[irows->iSort[j++]] = 0.0;
00638                         else
00639                                 i++;
00640         else
00641                 while (i < jc[cNum+1] && j < irows->length)
00642                         if (ir[i] == irows->number[irows->iSort[j]])
00643                                 col[irows->iSort[j++]] = alpha * val[i++];
00644                         else if (ir[i] > irows->number[irows->iSort[j]])
00645                                 col[irows->iSort[j++]] = 0.0;
00646                         else
00647                                 i++;
00648 
00649         /* fill in remaining zeros */
00650         while (j < irows->length)
00651                 col[irows->iSort[j++]] = 0.0;
00652 
00653         return SUCCESSFUL_RETURN;
00654 }
00655 
00656 returnValue SparseMatrix::times(int xN, real_t alpha, const real_t *x, int xLD,
00657                 real_t beta, real_t *y, int yLD) const
00658 {
00659         long i, j, k;
00660 
00661         if (isExactlyZero(beta))
00662                 for (k = 0; k < xN; k++)
00663                         for (j = 0; j < nRows; j++)
00664                                 y[j+k*yLD] = 0.0;
00665         else if (isExactlyMinusOne(beta))
00666                 for (k = 0; k < xN; k++)
00667                         for (j = 0; j < nRows; j++)
00668                                 y[j+k*yLD] = -y[j+k*yLD];
00669         else if (!isExactlyOne(beta))
00670                 for (k = 0; k < xN; k++)
00671                         for (j = 0; j < nRows; j++)
00672                                 y[j+k*yLD] *= beta;
00673 
00674         if (isExactlyOne(alpha))
00675                 for (k = 0; k < xN; k++)
00676                         for (j = 0; j < nCols; j++)
00677                                 for (i = jc[j]; i < jc[j+1]; i++)
00678                                         y[ir[i]+k*yLD] += val[i] * x[j+k*xLD];
00679         else if (isExactlyMinusOne(alpha))
00680                 for (k = 0; k < xN; k++)
00681                         for (j = 0; j < nCols; j++)
00682                                 for (i = jc[j]; i < jc[j+1]; i++)
00683                                         y[ir[i]+k*yLD] -= val[i] * x[j+k*xLD];
00684         else
00685                 for (k = 0; k < xN; k++)
00686                         for (j = 0; j < nCols; j++)
00687                                 for (i = jc[j]; i < jc[j+1]; i++)
00688                                         y[ir[i]+k*yLD] += alpha * val[i] * x[j+k*xLD];
00689 
00690         return SUCCESSFUL_RETURN;
00691 }
00692 
00693 returnValue SparseMatrix::transTimes(int xN, real_t alpha, const real_t *x, int xLD,
00694                 real_t beta, real_t *y, int yLD) const
00695 {
00696         long i, j, k;
00697 
00698         if (isExactlyZero(beta))
00699                 for (k = 0; k < xN; k++)
00700                         for (j = 0; j < nCols; j++)
00701                                 y[j+k*yLD] = 0.0;
00702         else if (isExactlyMinusOne(beta))
00703                 for (k = 0; k < xN; k++)
00704                         for (j = 0; j < nCols; j++)
00705                                 y[j+k*yLD] = -y[j+k*yLD];
00706         else if (!isExactlyOne(beta))
00707                 for (k = 0; k < xN; k++)
00708                         for (j = 0; j < nCols; j++)
00709                                 y[j+k*yLD] *= beta;
00710 
00711         if (isExactlyOne(alpha))
00712                 for (k = 0; k < xN; k++)
00713                         for (j = 0; j < nCols; j++)
00714                                 for (i = jc[j]; i < jc[j+1]; i++)
00715                                         y[j+k*yLD] += val[i] * x[ir[i]+k*xLD];
00716         else if (isExactlyMinusOne(alpha))
00717                 for (k = 0; k < xN; k++)
00718                         for (j = 0; j < nCols; j++)
00719                                 for (i = jc[j]; i < jc[j+1]; i++)
00720                                         y[j+k*yLD] -= val[i] * x[ir[i]+k*xLD];
00721         else
00722                 for (k = 0; k < xN; k++)
00723                         for (j = 0; j < nCols; j++)
00724                                 for (i = jc[j]; i < jc[j+1]; i++)
00725                                         y[j+k*yLD] += alpha * val[i] * x[ir[i]+k*xLD];
00726 
00727         return SUCCESSFUL_RETURN;
00728 }
00729 
00730 returnValue SparseMatrix::times(const Indexlist* const irows, const Indexlist* const icols,
00731                 int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD,
00732                 BooleanType yCompr) const
00733 {
00734         long i, j, k, l, col;
00735 
00736         if (yCompr == BT_TRUE)
00737         {
00738                 if (isExactlyZero(beta))
00739                         for (k = 0; k < xN; k++)
00740                                 for (j = 0; j < irows->length; j++)
00741                                         y[j+k*yLD] = 0.0;
00742                 else if (isExactlyMinusOne(beta))
00743                         for (k = 0; k < xN; k++)
00744                                 for (j = 0; j < irows->length; j++)
00745                                         y[j+k*yLD] = -y[j+k*yLD];
00746                 else if (!isExactlyOne(beta))
00747                         for (k = 0; k < xN; k++)
00748                                 for (j = 0; j < irows->length; j++)
00749                                         y[j+k*yLD] *= beta;
00750 
00751                 if (icols == 0)
00752                         if (isExactlyOne(alpha))
00753                                 for (l = 0; l < nCols; l++)
00754                                 {
00755                                         i = jc[l];
00756                                         j = 0;
00757                                         while (i < jc[l+1] && j < irows->length)
00758                                                 if (ir[i] == irows->number[irows->iSort[j]])
00759                                                 {
00760                                                         for (k = 0; k < xN; k++)
00761                                                                 y[k*yLD+irows->iSort[j]] += val[i] * x[k*xLD+l];
00762                                                         i++, j++;
00763                                                 }
00764                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00765                                                 else i++;
00766                                 }
00767                         else if (isExactlyMinusOne(alpha))
00768                                 for (l = 0; l < nCols; l++)
00769                                 {
00770                                         i = jc[l];
00771                                         j = 0;
00772                                         while (i < jc[l+1] && j < irows->length)
00773                                                 if (ir[i] == irows->number[irows->iSort[j]])
00774                                                 {
00775                                                         for (k = 0; k < xN; k++)
00776                                                                 y[k*yLD+irows->iSort[j]] -= val[i] * x[k*xLD+l];
00777                                                         i++, j++;
00778                                                 }
00779                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00780                                                 else i++;
00781                                 }
00782                         else
00783                                 for (l = 0; l < nCols; l++)
00784                                 {
00785                                         i = jc[l];
00786                                         j = 0;
00787                                         while (i < jc[l+1] && j < irows->length)
00788                                                 if (ir[i] == irows->number[irows->iSort[j]])
00789                                                 {
00790                                                         for (k = 0; k < xN; k++)
00791                                                                 y[k*yLD+irows->iSort[j]] += alpha * val[i] * x[k*xLD+l];
00792                                                         i++, j++;
00793                                                 }
00794                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00795                                                 else i++;
00796                                 }
00797                 else /* icols != 0 */
00798                         if (isExactlyOne(alpha))
00799                                 for (l = 0; l < icols->length; l++)
00800                                 {
00801                                         col = icols->iSort[l];
00802                                         i = jc[icols->number[col]];
00803                                         j = 0;
00804                                         while (i < jc[icols->number[col]+1] && j < irows->length)
00805                                                 if (ir[i] == irows->number[irows->iSort[j]])
00806                                                 {
00807                                                         for (k = 0; k < xN; k++)
00808                                                                 y[k*yLD+irows->iSort[j]] += val[i] * x[k*xLD+col];
00809                                                         i++, j++;
00810                                                 }
00811                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00812                                                 else i++;
00813                                 }
00814                         else if (isExactlyMinusOne(alpha))
00815                                 for (l = 0; l < icols->length; l++)
00816                                 {
00817                                         col = icols->iSort[l];
00818                                         i = jc[icols->number[col]];
00819                                         j = 0;
00820                                         while (i < jc[icols->number[col]+1] && j < irows->length)
00821                                                 if (ir[i] == irows->number[irows->iSort[j]])
00822                                                 {
00823                                                         for (k = 0; k < xN; k++)
00824                                                                 y[k*yLD+irows->iSort[j]] -= val[i] * x[k*xLD+col];
00825                                                         i++, j++;
00826                                                 }
00827                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00828                                                 else i++;
00829                                 }
00830                         else
00831                                 for (l = 0; l < icols->length; l++)
00832                                 {
00833                                         col = icols->iSort[l];
00834                                         i = jc[icols->number[col]];
00835                                         j = 0;
00836                                         while (i < jc[icols->number[col]+1] && j < irows->length)
00837                                                 if (ir[i] == irows->number[irows->iSort[j]])
00838                                                 {
00839                                                         for (k = 0; k < xN; k++)
00840                                                                 y[k*yLD+irows->iSort[j]] += alpha * val[i] * x[k*xLD+col];
00841                                                         i++, j++;
00842                                                 }
00843                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00844                                                 else i++;
00845                                 }
00846         }
00847         else /* y not compressed */
00848         {
00849                 if (isExactlyZero(beta))
00850                         for (k = 0; k < xN; k++)
00851                                 for (j = 0; j < irows->length; j++)
00852                                         y[irows->number[j]+k*yLD] = 0.0;
00853                 else if (isExactlyMinusOne(beta))
00854                         for (k = 0; k < xN; k++)
00855                                 for (j = 0; j < irows->length; j++)
00856                                         y[irows->number[j]+k*yLD] = -y[j+k*yLD];
00857                 else if (!isExactlyOne(beta))
00858                         for (k = 0; k < xN; k++)
00859                                 for (j = 0; j < irows->length; j++)
00860                                         y[irows->number[j]+k*yLD] *= beta;
00861 
00862                 if (icols == 0)
00863                         if (isExactlyOne(alpha))
00864                                 for (l = 0; l < nCols; l++)
00865                                 {
00866                                         i = jc[l];
00867                                         j = 0;
00868                                         while (i < jc[l+1] && j < irows->length)
00869                                                 if (ir[i] == irows->number[irows->iSort[j]])
00870                                                 {
00871                                                         for (k = 0; k < xN; k++)
00872                                                                 y[k*yLD+irows->number[irows->iSort[j]]] += val[i] * x[k*xLD+l];
00873                                                         i++, j++;
00874                                                 }
00875                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00876                                                 else i++;
00877                                 }
00878                         else if (isExactlyMinusOne(alpha))
00879                                 for (l = 0; l < nCols; l++)
00880                                 {
00881                                         i = jc[l];
00882                                         j = 0;
00883                                         while (i < jc[l+1] && j < irows->length)
00884                                                 if (ir[i] == irows->number[irows->iSort[j]])
00885                                                 {
00886                                                         for (k = 0; k < xN; k++)
00887                                                                 y[k*yLD+irows->number[irows->iSort[j]]] -= val[i] * x[k*xLD+l];
00888                                                         i++, j++;
00889                                                 }
00890                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00891                                                 else i++;
00892                                 }
00893                         else
00894                                 for (l = 0; l < nCols; l++)
00895                                 {
00896                                         i = jc[l];
00897                                         j = 0;
00898                                         while (i < jc[l+1] && j < irows->length)
00899                                                 if (ir[i] == irows->number[irows->iSort[j]])
00900                                                 {
00901                                                         for (k = 0; k < xN; k++)
00902                                                                 y[k*yLD+irows->number[irows->iSort[j]]] += alpha * val[i] * x[k*xLD+l];
00903                                                         i++, j++;
00904                                                 }
00905                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00906                                                 else i++;
00907                                 }
00908                 else /* icols != 0 */
00909                         if (isExactlyOne(alpha))
00910                                 for (l = 0; l < icols->length; l++)
00911                                 {
00912                                         col = icols->iSort[l];
00913                                         i = jc[icols->number[col]];
00914                                         j = 0;
00915                                         while (i < jc[icols->number[col]+1] && j < irows->length)
00916                                                 if (ir[i] == irows->number[irows->iSort[j]])
00917                                                 {
00918                                                         for (k = 0; k < xN; k++)
00919                                                                 y[k*yLD+irows->number[irows->iSort[j]]] += val[i] * x[k*xLD+col];
00920                                                         i++, j++;
00921                                                 }
00922                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00923                                                 else i++;
00924                                 }
00925                         else if (isExactlyMinusOne(alpha))
00926                                 for (l = 0; l < icols->length; l++)
00927                                 {
00928                                         col = icols->iSort[l];
00929                                         i = jc[icols->number[col]];
00930                                         j = 0;
00931                                         while (i < jc[icols->number[col]+1] && j < irows->length)
00932                                                 if (ir[i] == irows->number[irows->iSort[j]])
00933                                                 {
00934                                                         for (k = 0; k < xN; k++)
00935                                                                 y[k*yLD+irows->number[irows->iSort[j]]] -= val[i] * x[k*xLD+col];
00936                                                         i++, j++;
00937                                                 }
00938                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00939                                                 else i++;
00940                                 }
00941                         else
00942                                 for (l = 0; l < icols->length; l++)
00943                                 {
00944                                         col = icols->iSort[l];
00945                                         i = jc[icols->number[col]];
00946                                         j = 0;
00947                                         while (i < jc[icols->number[col]+1] && j < irows->length)
00948                                                 if (ir[i] == irows->number[irows->iSort[j]])
00949                                                 {
00950                                                         for (k = 0; k < xN; k++)
00951                                                                 y[k*yLD+irows->number[irows->iSort[j]]] += alpha * val[i] * x[k*xLD+col];
00952                                                         i++, j++;
00953                                                 }
00954                                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00955                                                 else i++;
00956                                 }
00957         }
00958         return SUCCESSFUL_RETURN;
00959 }
00960 
00961 returnValue SparseMatrix::transTimes(const Indexlist* const irows, const Indexlist* const icols,
00962                 int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
00963 {
00964         long i, j, k, l, col;
00965 
00966         if (isExactlyZero(beta))
00967                 for (k = 0; k < xN; k++)
00968                         for (j = 0; j < icols->length; j++)
00969                                 y[j+k*yLD] = 0.0;
00970         else if (isExactlyMinusOne(beta))
00971                 for (k = 0; k < xN; k++)
00972                         for (j = 0; j < icols->length; j++)
00973                                 y[j+k*yLD] = -y[j+k*yLD];
00974         else if (!isExactlyOne(beta))
00975                 for (k = 0; k < xN; k++)
00976                         for (j = 0; j < icols->length; j++)
00977                                 y[j+k*yLD] *= beta;
00978 
00979         if (isExactlyOne(alpha))
00980                 for (l = 0; l < icols->length; l++)
00981                 {
00982                         col = icols->iSort[l];
00983                         i = jc[icols->number[col]];
00984                         j = 0;
00985                         while (i < jc[icols->number[col]+1] && j < irows->length)
00986                                 if (ir[i] == irows->number[irows->iSort[j]])
00987                                 {
00988                                         for (k = 0; k < xN; k++)
00989                                                 y[k*yLD+col] += val[i] * x[k*xLD+irows->iSort[j]];
00990                                         i++, j++;
00991                                 }
00992                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
00993                                 else i++;
00994                 }
00995         else if (isExactlyMinusOne(alpha))
00996                 for (l = 0; l < icols->length; l++)
00997                 {
00998                         col = icols->iSort[l];
00999                         i = jc[icols->number[col]];
01000                         j = 0;
01001                         while (i < jc[icols->number[col]+1] && j < irows->length)
01002                                 if (ir[i] == irows->number[irows->iSort[j]])
01003                                 {
01004                                         for (k = 0; k < xN; k++)
01005                                                 y[k*yLD+col] -= val[i] * x[k*xLD+irows->iSort[j]];
01006                                         i++, j++;
01007                                 }
01008                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
01009                                 else i++;
01010                 }
01011         else
01012                 for (l = 0; l < icols->length; l++)
01013                 {
01014                         col = icols->iSort[l];
01015                         i = jc[icols->number[col]];
01016                         j = 0;
01017                         while (i < jc[icols->number[col]+1] && j < irows->length)
01018                                 if (ir[i] == irows->number[irows->iSort[j]])
01019                                 {
01020                                         for (k = 0; k < xN; k++)
01021                                                 y[k*yLD+col] += alpha * val[i] * x[k*xLD+irows->iSort[j]];
01022                                         i++, j++;
01023                                 }
01024                                 else if (ir[i] > irows->number[irows->iSort[j]]) j++;
01025                                 else i++;
01026                 }
01027 
01028         return SUCCESSFUL_RETURN;
01029 }
01030 
01031 returnValue SparseMatrix::addToDiag(real_t alpha)
01032 {
01033         long i;
01034 
01035         if (!isExactlyZero(alpha))
01036         {
01037                 for (i = 0; i < nRows && i < nCols; i++)
01038                         if (ir[jd[i]] == i) val[jd[i]] += alpha;
01039                         else return RET_NO_DIAGONAL_AVAILABLE;
01040         }
01041 
01042         return SUCCESSFUL_RETURN;
01043 }
01044 
01045 long *SparseMatrix::createDiagInfo()
01046 {
01047         long i, j;
01048 
01049         if (jd == 0) {
01050                 jd = new long[nCols];
01051 
01052                 for (j = 0; j < nCols; j++)
01053                 {
01054                         for (i = jc[j]; i < jc[j+1] && ir[i] < j; i++);
01055                         jd[j] = i;
01056                 }
01057         }
01058 
01059         return jd;
01060 }
01061 
01062 real_t *SparseMatrix::full() const
01063 {
01064         long i, j;
01065         real_t *v = new real_t[nRows*nCols];
01066 
01067         for (i = 0; i < nCols*nRows; i++)
01068                 v[i] = 0.0;
01069 
01070         for (j = 0; j < nCols; j++)
01071                 for (i = jc[j]; i < jc[j+1]; i++)
01072                         v[ir[i] * nCols + j] = val[i];
01073 
01074         return v;
01075 }
01076 
01077 
01078 returnValue SparseMatrix::print( ) const
01079 {
01080         return THROWERROR( RET_NOT_YET_IMPLEMENTED );
01081 }
01082 
01083 
01084 
01085 Matrix *SymSparseMat::duplicate() const
01086 {
01087         /* "same" as duplicate() in SparseMatrix */
01088         long i, length = jc[nCols];
01089         SymSparseMat *dupl = new SymSparseMat;
01090 
01091         dupl->nRows = nRows;
01092         dupl->nCols = nCols;
01093         dupl->ir = new long[length];
01094         dupl->jc = new long[nCols+1];
01095         dupl->jd = new long[nCols];
01096         dupl->val = new real_t[length];
01097 
01098         for (i = 0; i < length; i++) dupl->ir[i] = ir[i];
01099         for (i = 0; i <= nCols; i++) dupl->jc[i] = jc[i];
01100         for (i = 0; i < nCols; i++) dupl->jd[i] = jd[i];
01101         for (i = 0; i < length; i++) dupl->val[i] = val[i];
01102         
01103         dupl->doFreeMemory( );
01104 
01105         return dupl;
01106 }
01107 
01108 returnValue SymSparseMat::bilinear(const Indexlist* const icols,
01109                 int xN, const real_t *x, int xLD, real_t *y, int yLD) const
01110 {
01111         int i, j, k, l, idx, row, col;
01112 
01113         /* clear output */
01114         for (i = 0; i < xN*xN; i++)
01115                 y[i] = 0.0;
01116 
01117         /* compute lower triangle */
01118         for (l = 0; l < icols->length; l++)
01119         {
01120                 col = icols->number[icols->iSort[l]];
01121                 idx = jd[col];
01122                 k = 0;
01123                 while (idx < jc[col+1] && k < icols->length)
01124                 {
01125                         row = icols->number[icols->iSort[k]];
01126                         if (ir[idx] == row)
01127                         {
01128                                 /* TODO: It is possible to formulate this as DSYR and DSYR2
01129                                  * operations. */
01130                                 if (row == col) /* diagonal element */
01131                                         for (i = 0; i < xN; i++)
01132                                                 for (j = i; j < xN; j++)
01133                                                         y[i*yLD+j] += val[idx] * x[i*xLD+col] * x[j*xLD+col];
01134                                 else /* subdiagonal elements */
01135                                         for (i = 0; i < xN; i++)
01136                                                 for (j = i; j < xN; j++)
01137                                                         y[i*yLD+j] += val[idx] * (x[i*xLD+col] * x[j*xLD+row] + x[i*xLD+row] * x[j*xLD+col]);
01138                                 idx++, k++;
01139                         }
01140                         else if (ir[idx] > row) k++;
01141                         else idx++;
01142                 }
01143         }
01144 
01145         /* fill upper triangle */
01146         for (i = 0; i < xN; i++)
01147                 for (j = i; j < xN; j++)
01148                         y[j*yLD+i] = y[i*yLD+j];
01149 
01150         return SUCCESSFUL_RETURN;
01151 }
01152 
01153 
01154 END_NAMESPACE_QPOASES
01155 
01156 
01157 /*
01158  *      end of file
01159  */


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:02