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-2008 by Hans Joachim Ferreau et al. All rights reserved.
00006  *
00007  *      qpOASES is free software; you can redistribute it and/or
00008  *      modify it under the terms of the GNU Lesser General Public
00009  *      License as published by the Free Software Foundation; either
00010  *      version 2.1 of the License, or (at your option) any later version.
00011  *
00012  *      qpOASES is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00015  *      Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with qpOASES; if not, write to the Free Software
00019  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00020  *
00021  */
00022 
00023 
00035 #include <Bounds.hpp>
00036 
00037 
00038 
00039 /*****************************************************************************
00040  *  P U B L I C                                                              *
00041  *****************************************************************************/
00042 
00043 
00044 /*
00045  *      B o u n d s
00046  */
00047 Bounds::Bounds( ) :     SubjectTo( ),
00048                                         nV( 0 ),
00049                                         nFV( 0 ),
00050                                         nBV( 0 ),
00051                                         nUV( 0 )
00052 {
00053 }
00054 
00055 
00056 /*
00057  *      B o u n d s
00058  */
00059 Bounds::Bounds( const Bounds& rhs ) :   SubjectTo( rhs ),
00060                                                                                 nV( rhs.nV ),
00061                                                                                 nFV( rhs.nFV ),
00062                                                                                 nBV( rhs.nBV ),
00063                                                                                 nUV( rhs.nUV )
00064 {
00065         free  = rhs.free;
00066         fixed = rhs.fixed;
00067 }
00068 
00069 
00070 /*
00071  *      ~ B o u n d s
00072  */
00073 Bounds::~Bounds( )
00074 {
00075 }
00076 
00077 
00078 /*
00079  *      o p e r a t o r =
00080  */
00081 Bounds& Bounds::operator=( const Bounds& rhs )
00082 {
00083         if ( this != &rhs )
00084         {
00085                 SubjectTo::operator=( rhs );
00086 
00087                 nV  = rhs.nV;
00088                 nFV = rhs.nFV;
00089                 nBV = rhs.nBV;
00090                 nUV = rhs.nUV;
00091 
00092                 free  = rhs.free;
00093                 fixed = rhs.fixed;
00094         }
00095 
00096         return *this;
00097 }
00098 
00099 
00100 /*
00101  *      i n i t
00102  */
00103 returnValue Bounds::init( int n )
00104 {
00105         nV = n;
00106         nFV = 0;
00107         nBV = 0;
00108         nUV = 0;
00109 
00110         free.init( );
00111         fixed.init( );
00112 
00113         return SubjectTo::init( n );
00114 }
00115 
00116 
00117 /*
00118  *      s e t u p B o u n d
00119  */
00120 returnValue Bounds::setupBound( int _number, SubjectToStatus _status
00121                                                                 )
00122 {
00123         /* consistency check */
00124         if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
00125                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00126         
00127         /* Add bound index to respective index list. */
00128         switch ( _status )
00129         {
00130                 case ST_INACTIVE:
00131                         if ( this->addIndex( this->getFree( ),_number,_status ) != SUCCESSFUL_RETURN )
00132                                 return THROWERROR( RET_SETUP_BOUND_FAILED );
00133                         break;
00134 
00135                 case ST_LOWER:
00136                         if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
00137                                 return THROWERROR( RET_SETUP_BOUND_FAILED );
00138                         break;
00139 
00140                 case ST_UPPER:
00141                         if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
00142                                 return THROWERROR( RET_SETUP_BOUND_FAILED );
00143                         break;
00144 
00145                 default:
00146                         return THROWERROR( RET_INVALID_ARGUMENTS );
00147         }
00148 
00149         return SUCCESSFUL_RETURN;
00150 }
00151 
00152 
00153 /*
00154  *      s e t u p A l l F r e e
00155  */
00156 returnValue Bounds::setupAllFree( )
00157 {
00158         int i;
00159 
00160         /* 1) Place unbounded variables at the beginning of the index list of free variables. */
00161         for( i=0; i<nV; ++i )
00162         {
00163                 if ( getType( i ) == ST_UNBOUNDED )
00164                 {
00165                         if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
00166                                         return THROWERROR( RET_SETUP_BOUND_FAILED );
00167                 }
00168         }
00169 
00170         /* 2) Add remaining (i.e. bounded but possibly free) variables to the index list of free variables. */
00171         for( i=0; i<nV; ++i )
00172         {
00173                 if ( getType( i ) == ST_BOUNDED )
00174                 {
00175                         if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
00176                                 return THROWERROR( RET_SETUP_BOUND_FAILED );
00177                 }
00178         }
00179 
00180         /* 3) Place implicitly fixed variables at the end of the index list of free variables. */
00181         for( i=0; i<nV; ++i )
00182         {
00183                 if ( getType( i ) == ST_EQUALITY )
00184                 {
00185                         if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
00186                                 return THROWERROR( RET_SETUP_BOUND_FAILED );
00187                 }
00188         }
00189 
00190         return SUCCESSFUL_RETURN;
00191 }
00192 
00193 
00194 /*
00195  *      m o v e F i x e d T o F r e e
00196  */
00197 returnValue Bounds::moveFixedToFree( int _number )
00198 {
00199         /* consistency check */
00200         if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
00201                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00202 
00203         /* Move index from indexlist of fixed variables to that of free ones. */
00204         if ( this->removeIndex( this->getFixed( ),_number ) != SUCCESSFUL_RETURN )
00205                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00206 
00207         if ( this->addIndex( this->getFree( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
00208                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00209 
00210         return SUCCESSFUL_RETURN;
00211 }
00212 
00213 
00214 /*
00215  *      m o v e F r e e T o F i x e d
00216  */
00217 returnValue Bounds::moveFreeToFixed(    int _number, SubjectToStatus _status
00218                                                                                 )
00219 {
00220         /* consistency check */
00221         if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
00222                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00223 
00224         /* Move index from indexlist of free variables to that of fixed ones. */
00225         if ( this->removeIndex( this->getFree( ),_number ) != SUCCESSFUL_RETURN )
00226                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00227 
00228         if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
00229                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00230 
00231         return SUCCESSFUL_RETURN;
00232 }
00233 
00234 
00235 /*
00236  *      s w a p F r e e
00237  */
00238 returnValue Bounds::swapFree(   int number1, int number2
00239                                                                 )
00240 {
00241         /* consistency check */
00242         if ( ( number1 < 0 ) || ( number1 >= getNV( ) ) || ( number2 < 0 ) || ( number2 >= getNV( ) ) )
00243                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00244 
00245         /* Swap index within indexlist of free variables. */
00246         return this->swapIndex( this->getFree( ),number1,number2 );
00247 }
00248 
00249 
00250 /*
00251  *      end of file
00252  */


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