Bounds.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 
00036 #include <qpOASES/Bounds.hpp>
00037 
00038 
00039 BEGIN_NAMESPACE_QPOASES
00040 
00041 
00042 /*****************************************************************************
00043  *  P U B L I C                                                              *
00044  *****************************************************************************/
00045 
00046 
00047 /*
00048  *      B o u n d s
00049  */
00050 Bounds::Bounds( ) : SubjectTo( )
00051 {
00052 }
00053 
00054 
00055 /*
00056  *      B o u n d s
00057  */
00058 Bounds::Bounds( int _n ) : SubjectTo( _n )
00059 {
00060         init( _n );
00061 }
00062 
00063 
00064 /*
00065  *      B o u n d s
00066  */
00067 Bounds::Bounds( const Bounds& rhs ) : SubjectTo( rhs )
00068 {
00069         copy( rhs );
00070 }
00071 
00072 
00073 /*
00074  *      ~ B o u n d s
00075  */
00076 Bounds::~Bounds( )
00077 {
00078         clear( );
00079 }
00080 
00081 
00082 /*
00083  *      o p e r a t o r =
00084  */
00085 Bounds& Bounds::operator=( const Bounds& rhs )
00086 {
00087         if ( this != &rhs )
00088         {
00089                 clear( );
00090                 SubjectTo::operator=( rhs );
00091                 copy( rhs );
00092         }
00093 
00094         return *this;
00095 }
00096 
00097 
00098 
00099 /*
00100  *      i n i t
00101  */
00102 returnValue Bounds::init(       int _n
00103                                                         )
00104 {
00105         if ( _n < 0 )
00106                 return THROWERROR( RET_INVALID_ARGUMENTS );
00107 
00108         clear( );
00109 
00110         if ( _n > 0 )
00111         {
00112                 freee.init( _n );
00113                 fixed.init( _n );
00114         }
00115 
00116         return SubjectTo::init( _n );
00117 }
00118 
00119 
00120 
00121 /*
00122  *      s e t u p B o u n d
00123  */
00124 returnValue Bounds::setupBound( int number, SubjectToStatus _status
00125                                                                 )
00126 {
00127         /* consistency check */
00128         if ( ( number < 0 ) || ( number >= n ) )
00129                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00130 
00131         /* Add bound index to respective index list. */
00132         switch ( _status )
00133         {
00134                 case ST_INACTIVE:
00135                         if ( this->addIndex( this->getFree( ),number,_status ) != SUCCESSFUL_RETURN )
00136                                 return THROWERROR( RET_SETUP_BOUND_FAILED );
00137                         break;
00138 
00139                 case ST_LOWER:
00140                         if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
00141                                 return THROWERROR( RET_SETUP_BOUND_FAILED );
00142                         break;
00143 
00144                 case ST_UPPER:
00145                         if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
00146                                 return THROWERROR( RET_SETUP_BOUND_FAILED );
00147                         break;
00148 
00149                 default:
00150                         return THROWERROR( RET_INVALID_ARGUMENTS );
00151         }
00152 
00153         return SUCCESSFUL_RETURN;
00154 }
00155 
00156 
00157 /*
00158  *      s e t u p A l l F r e e
00159  */
00160 returnValue Bounds::setupAllFree( )
00161 {
00162         return setupAll( ST_INACTIVE );
00163 }
00164 
00165 
00166 /*
00167  *      s e t u p A l l L o w e r
00168  */
00169 returnValue Bounds::setupAllLower( )
00170 {
00171         return setupAll( ST_LOWER );
00172 }
00173 
00174 
00175 /*
00176  *      s e t u p A l l U p p e r
00177  */
00178 returnValue Bounds::setupAllUpper( )
00179 {
00180         return setupAll( ST_UPPER );
00181 }
00182 
00183 
00184 /*
00185  *      m o v e F i x e d T o F r e e
00186  */
00187 returnValue Bounds::moveFixedToFree( int number )
00188 {
00189         /* consistency check */
00190         if ( ( number < 0 ) || ( number >= n ) )
00191                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00192 
00193         /* Move index from indexlist of fixed variables to that of free ones. */
00194         if ( this->removeIndex( this->getFixed( ),number ) != SUCCESSFUL_RETURN )
00195                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00196 
00197         if ( this->addIndex( this->getFree( ),number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
00198                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00199 
00200         return SUCCESSFUL_RETURN;
00201 }
00202 
00203 
00204 /*
00205  *      m o v e F r e e T o F i x e d
00206  */
00207 returnValue Bounds::moveFreeToFixed(    int number, SubjectToStatus _status
00208                                                                                 )
00209 {
00210         /* consistency check */
00211         if ( ( number < 0 ) || ( number >= n ) )
00212                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00213 
00214         /* Move index from indexlist of free variables to that of fixed ones. */
00215         if ( this->removeIndex( this->getFree( ),number ) != SUCCESSFUL_RETURN )
00216                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00217 
00218         if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
00219                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00220 
00221         return SUCCESSFUL_RETURN;
00222 }
00223 
00224 
00225 /*
00226  *      f l i p F i x e d
00227  */
00228 returnValue Bounds::flipFixed( int number )
00229 {
00230         /* consistency check */
00231         if ( ( number < 0 ) || ( number >= n ) )
00232                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00233 
00234         if ( status != 0 )
00235                 switch (status[number])
00236                 {
00237                         case ST_LOWER: status[number] = ST_UPPER; break;
00238                         case ST_UPPER: status[number] = ST_LOWER; break;
00239                         default: return THROWERROR( RET_MOVING_BOUND_FAILED );
00240                 }
00241 
00242         return SUCCESSFUL_RETURN;
00243 }
00244 
00245 
00246 /*
00247  *      s w a p F r e e
00248  */
00249 returnValue Bounds::swapFree(   int number1, int number2
00250                                                                 )
00251 {
00252         /* consistency check */
00253         if ( ( number1 < 0 ) || ( number1 >= n ) || ( number2 < 0 ) || ( number2 >= n ) )
00254                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00255 
00256         /* Swap index within indexlist of free variables. */
00257         return this->swapIndex( this->getFree( ),number1,number2 );
00258 }
00259 
00260 
00261 /*
00262  *      s h i f t
00263  */
00264 returnValue Bounds::shift(      int offset )
00265 {
00266         int i;
00267 
00268         /* consistency check */
00269         if ( ( offset == 0 ) || ( n <= 1 ) )
00270                 return SUCCESSFUL_RETURN;
00271 
00272         if ( ( offset < 0 ) || ( offset > n/2 ) )
00273                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00274 
00275         if ( ( n % offset ) != 0 )
00276                 return THROWERROR( RET_INVALID_ARGUMENTS );
00277 
00278 
00279         /* 1) Shift types and status. */
00280         for( i=0; i<n-offset; ++i )
00281         {
00282                 setType( i,getType( i+offset ) );
00283                 setStatus( i,getStatus( i+offset ) );
00284         }
00285 
00286         /* 2) Construct shifted index lists of free and fixed variables. */
00287         Indexlist shiftedFreee( n );
00288         Indexlist shiftedFixed( n );
00289 
00290         for( i=0; i<n; ++i )
00291         {
00292                 switch ( getStatus( i ) )
00293                 {
00294                         case ST_INACTIVE:
00295                                 if ( shiftedFreee.addNumber( i ) != SUCCESSFUL_RETURN )
00296                                         return THROWERROR( RET_SHIFTING_FAILED );
00297                                 break;
00298 
00299                         case ST_LOWER:
00300                                 if ( shiftedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
00301                                         return THROWERROR( RET_SHIFTING_FAILED );
00302                                 break;
00303 
00304                         case ST_UPPER:
00305                                 if ( shiftedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
00306                                         return THROWERROR( RET_SHIFTING_FAILED );
00307                                 break;
00308 
00309                         default:
00310                                 return THROWERROR( RET_SHIFTING_FAILED );
00311                 }
00312         }
00313 
00314         /* 3) Assign shifted index list. */
00315         freee = shiftedFreee;
00316         fixed = shiftedFixed;
00317 
00318         return SUCCESSFUL_RETURN;
00319 }
00320 
00321 
00322 /*
00323  *      r o t a t e
00324  */
00325 returnValue Bounds::rotate( int offset )
00326 {
00327         int i;
00328 
00329         /* consistency check */
00330         if ( ( offset == 0 ) || ( offset == n ) || ( n <= 1 ) )
00331                 return SUCCESSFUL_RETURN;
00332 
00333         if ( ( offset < 0 ) || ( offset > n ) )
00334                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00335 
00336 
00337         /* 1) Rotate types and status. */
00338         SubjectToType*   typeTmp   = new SubjectToType[offset];
00339         SubjectToStatus* statusTmp = new SubjectToStatus[offset];
00340 
00341         for( i=0; i<offset; ++i )
00342         {
00343                 typeTmp[i] = getType( i );
00344                 statusTmp[i] = getStatus( i );
00345         }
00346 
00347         for( i=0; i<n-offset; ++i )
00348         {
00349                 setType( i,getType( i+offset ) );
00350                 setStatus( i,getStatus( i+offset ) );
00351         }
00352 
00353         for( i=n-offset; i<n; ++i )
00354         {
00355                 setType( i,typeTmp[i-n+offset] );
00356                 setStatus( i,statusTmp[i-n+offset] );
00357         }
00358 
00359         delete[] statusTmp; delete[] typeTmp;
00360 
00361         /* 2) Construct shifted index lists of free and fixed variables. */
00362         Indexlist rotatedFreee( n );
00363         Indexlist rotatedFixed( n );
00364 
00365         for( i=0; i<n; ++i )
00366         {
00367                 switch ( getStatus( i ) )
00368                 {
00369                         case ST_INACTIVE:
00370                                 if ( rotatedFreee.addNumber( i ) != SUCCESSFUL_RETURN )
00371                                         return THROWERROR( RET_ROTATING_FAILED );
00372                                 break;
00373 
00374                         case ST_LOWER:
00375                                 if ( rotatedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
00376                                         return THROWERROR( RET_ROTATING_FAILED );
00377                                 break;
00378 
00379                         case ST_UPPER:
00380                                 if ( rotatedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
00381                                         return THROWERROR( RET_ROTATING_FAILED );
00382                                 break;
00383 
00384                         default:
00385                                 return THROWERROR( RET_ROTATING_FAILED );
00386                 }
00387         }
00388 
00389         /* 3) Assign shifted index list. */
00390         freee = rotatedFreee;
00391         fixed = rotatedFixed;
00392 
00393         return SUCCESSFUL_RETURN;
00394 }
00395 
00396 
00397 /*
00398  *      p r i n t
00399  */
00400 returnValue Bounds::print( )
00401 {
00402         if ( n == 0 )
00403                 return SUCCESSFUL_RETURN;
00404 
00405         #ifndef __XPCTARGET__
00406         #ifndef __DSPACE__
00407         char myPrintfString[160];
00408 
00409         int nFR = getNFR( );
00410         int nFX = getNFX( );
00411 
00412         int* FR_idx;
00413         getFree( )->getNumberArray( &FR_idx );
00414 
00415         int* FX_idx;
00416         getFixed( )->getNumberArray( &FX_idx );
00417 
00418         snprintf( myPrintfString,160,"Bounds object comprising %d variables (%d free, %d fixed):\n",n,nFR,nFX );
00419         myPrintf( myPrintfString );
00420 
00421         REFER_NAMESPACE_QPOASES print( FR_idx,nFR,"free " );
00422         REFER_NAMESPACE_QPOASES print( FX_idx,nFX,"fixed" );
00423 
00424         #endif
00425         #endif
00426 
00427         return SUCCESSFUL_RETURN;
00428 }
00429 
00430 
00431 
00432 /*****************************************************************************
00433  *  P R O T E C T E D                                                        *
00434  *****************************************************************************/
00435 
00436 /*
00437  *      c l e a r
00438  */
00439 returnValue Bounds::clear( )
00440 {
00441         return SUCCESSFUL_RETURN;
00442 }
00443 
00444 
00445 /*
00446  *      c o p y
00447  */
00448 returnValue Bounds::copy(       const Bounds& rhs
00449                                                         )
00450 {
00451         freee = rhs.freee;
00452         fixed = rhs.fixed;
00453 
00454         return SUCCESSFUL_RETURN;
00455 }
00456 
00457 
00458 
00459 /*
00460  *      s e t u p A l l
00461  */
00462 returnValue Bounds::setupAll( SubjectToStatus _status )
00463 {
00464         int i;
00465 
00466         /* 1) Place unbounded variables at the beginning of the index list of free variables. */
00467         for( i=0; i<n; ++i )
00468         {
00469                 if ( getType( i ) == ST_UNBOUNDED )
00470                 {
00471                         if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
00472                                         return THROWERROR( RET_SETUP_BOUND_FAILED );
00473                 }
00474         }
00475 
00476         /* 2) Add remaining (i.e. bounded but possibly free) variables to the index list of free variables. */
00477         for( i=0; i<n; ++i )
00478         {
00479                 if ( getType( i ) == ST_BOUNDED )
00480                 {
00481                         if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
00482                                 return THROWERROR( RET_SETUP_BOUND_FAILED );
00483                 }
00484         }
00485 
00486         /* 3) Place implicitly fixed variables at the end of the index list of free variables. */
00487         for( i=0; i<n; ++i )
00488         {
00489                 if ( getType( i ) == ST_EQUALITY )
00490                 {
00491                         if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
00492                                 return THROWERROR( RET_SETUP_BOUND_FAILED );
00493                 }
00494         }
00495 
00496         /* 4) Moreover, add all bounds of unknown type. */
00497         for( i=0; i<n; ++i )
00498         {
00499                 if ( getType( i ) == ST_UNKNOWN )
00500                 {
00501                         if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
00502                                 return THROWERROR( RET_SETUP_BOUND_FAILED );
00503                 }
00504         }
00505 
00506         return SUCCESSFUL_RETURN;
00507 }
00508 
00509 
00510 END_NAMESPACE_QPOASES
00511 
00512 
00513 /*
00514  *      end of file
00515  */


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