Indexlist.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 <Indexlist.hpp>
00036 
00037 
00038 /*****************************************************************************
00039  *  P U B L I C                                                              *
00040  *****************************************************************************/
00041 
00042 
00043 /*
00044  *      I n d e x l i s t
00045  */
00046 Indexlist::Indexlist( ) :       length( 0 ),
00047                                                         first( -1 ),
00048                                                         last( -1 ),
00049                                                         lastusedindex( -1 ),
00050                                                         physicallength( INDEXLISTFACTOR*(NVMAX+NCMAX) )
00051 {
00052         int i;
00053 
00054         for( i=0; i<physicallength; ++i )
00055         {
00056                 number[i] = -1;
00057                 next[i] = -1;
00058                 previous[i] = -1;
00059         }
00060 }
00061 
00062 
00063 /*
00064  *      I n d e x l i s t
00065  */
00066 Indexlist::Indexlist( const Indexlist& rhs ) :  length( rhs.length ),
00067                                                                                                 first( rhs.first ),
00068                                                                                                 last( rhs.last ),
00069                                                                                                 lastusedindex( rhs.lastusedindex ),
00070                                                                                                 physicallength( rhs.physicallength )
00071 {
00072         int i;
00073 
00074         for( i=0; i<physicallength; ++i )
00075         {
00076                 number[i] = rhs.number[i];
00077                 next[i] = rhs.next[i];
00078                 previous[i] = rhs.previous[i];
00079         }
00080 }
00081 
00082 
00083 /*
00084  *      ~ I n d e x l i s t
00085  */
00086 Indexlist::~Indexlist( )
00087 {
00088 }
00089 
00090 
00091 /*
00092  *      o p e r a t o r =
00093  */
00094 Indexlist& Indexlist::operator=( const Indexlist& rhs )
00095 {
00096         int i;
00097 
00098         if ( this != &rhs )
00099         {
00100                 length = rhs.length;
00101                 first = rhs.first;
00102                 last = rhs.last;
00103                 lastusedindex = rhs.lastusedindex;
00104                 physicallength = rhs.physicallength;
00105 
00106                 for( i=0; i<physicallength; ++i )
00107                 {
00108                         number[i] = rhs.number[i];
00109                         next[i] = rhs.next[i];
00110                         previous[i] = rhs.previous[i];
00111                 }
00112         }
00113 
00114         return *this;
00115 }
00116 
00117 
00118 /*
00119  *      i n i t
00120  */
00121 returnValue Indexlist::init( )
00122 {
00123         int i;
00124 
00125         length = 0;
00126         first = -1;
00127         last = -1;
00128         lastusedindex = -1;
00129         physicallength = INDEXLISTFACTOR*(NVMAX+NCMAX);
00130 
00131         for( i=0; i<physicallength; ++i )
00132         {
00133                 number[i] = -1;
00134                 next[i] = -1;
00135                 previous[i] = -1;
00136         }
00137 
00138         return SUCCESSFUL_RETURN;
00139 }
00140 
00141 
00142 /*
00143  *      g e t N u m b e r A r r a y
00144  */
00145 returnValue Indexlist::getNumberArray( int* const numberarray ) const
00146 {
00147         int i;
00148         int n = first;
00149 
00150         /* Run trough indexlist and store numbers in numberarray. */
00151         for( i=0; i<length; ++i )
00152         {
00153                 if ( ( n >= 0 ) && ( number[n] >= 0 ) )
00154                         numberarray[i] = number[n];
00155                 else
00156                         return THROWERROR( RET_INDEXLIST_CORRUPTED );
00157 
00158                 n = next[n];
00159         }
00160 
00161         return SUCCESSFUL_RETURN;
00162 }
00163 
00164 
00165 /*
00166  *      g e t I n d e x
00167  */
00168 int Indexlist::getIndex( int givennumber ) const
00169 {
00170         int i;
00171         int n = first;
00172         int index = -1; /* return -1 by default */
00173 
00174         /* Run trough indexlist until number is found, if so return it index. */
00175         for ( i=0; i<length; ++i )
00176         {
00177                 if ( number[n] == givennumber )
00178                 {
00179                         index = i;
00180                         break;
00181                 }
00182 
00183                 n = next[n];
00184         }
00185 
00186         return index;
00187 }
00188 
00189 
00190 /*
00191  *      g e t P h y s i c a l I n d e x
00192  */
00193 int Indexlist::getPhysicalIndex( int givennumber ) const
00194 {
00195         int i;
00196         int n = first;
00197         int index = -1; /* return -1 by default */
00198 
00199         /* Run trough indexlist until number is found, if so return it physicalindex. */
00200         for ( i=0; i<length; ++i )
00201         {
00202                 if ( number[n] == givennumber )
00203                 {
00204                         index = n;
00205                         break;
00206                 }
00207 
00208                 n = next[n];
00209         }
00210 
00211         return index;
00212 }
00213 
00214 
00215 /*
00216  *      a d d N u m b e r
00217  */
00218 returnValue Indexlist::addNumber( int addnumber )
00219 {
00220         int i;
00221 
00222         if ( lastusedindex+1 < physicallength )
00223         {
00224                 /* If there is enough storage, add number to indexlist. */
00225                 ++lastusedindex;
00226                 number[lastusedindex] = addnumber;
00227                 next[lastusedindex] = 0;
00228 
00229                 if ( length == 0 )
00230                 {
00231                         first = lastusedindex;
00232                         previous[lastusedindex] = 0;
00233                 }
00234                 else
00235                 {
00236                         next[last] = lastusedindex;
00237                         previous[lastusedindex] = last;
00238                 }
00239 
00240                 last = lastusedindex;
00241                 ++length;
00242 
00243                 return SUCCESSFUL_RETURN;
00244         }
00245         else
00246         {
00247                 /* Rearrangement of index list necessary! */
00248                 if ( length == physicallength )
00249                         return THROWERROR( RET_INDEXLIST_EXCEEDS_MAX_LENGTH );
00250                 else
00251                 {
00252                         int numberArray[NVMAX+NCMAX];
00253                         getNumberArray( numberArray );
00254 
00255                         /* copy existing elements */
00256                         for ( i=0; i<length; ++i )
00257                         {
00258                                 number[i] = numberArray[i];
00259                                 next[i] = i+1;
00260                                 previous[i] = i-1;
00261                         }
00262 
00263                         /* add new number at end of list */
00264                         number[length] = addnumber;
00265                         next[length] = -1;
00266                         previous[length] = length-1;
00267 
00268                         /* and set remaining entries to empty */
00269                         for ( i=length+1; i<physicallength; ++i )
00270                         {
00271                                 number[i] = -1;
00272                                 next[i] = -1;
00273                                 previous[i] = -1;
00274                         }
00275 
00276                         first = 0;
00277                         last = length;
00278                         lastusedindex = length;
00279                         ++length;
00280 
00281                         return THROWWARNING( RET_INDEXLIST_MUST_BE_REORDERD );
00282                 }
00283         }
00284 }
00285 
00286 
00287 /*
00288  *      r e m o v e N u m b e r
00289  */
00290 returnValue Indexlist::removeNumber( int removenumber )
00291 {
00292         int i = getPhysicalIndex( removenumber );
00293 
00294         /* nothing to be done if number is not contained in index set */
00295         if ( i < 0 )
00296                 return SUCCESSFUL_RETURN;
00297 
00298         int p = previous[i];
00299         int n = next[i];
00300 
00301         if ( i == last )
00302                 last = p;
00303         else
00304                 previous[n] = p;
00305 
00306         if ( i == first )
00307                 first = n;
00308         else
00309                 next[p] = n;
00310 
00311         number[i] = -1;
00312         next[i] = -1;
00313         previous[i] = -1;
00314         --length;
00315 
00316         return SUCCESSFUL_RETURN;
00317 }
00318 
00319 
00320 /*
00321  *      s w a p N u m b e r s
00322  */
00323 returnValue Indexlist::swapNumbers( int number1, int number2 )
00324 {
00325         int index1 = getPhysicalIndex( number1 );
00326         int index2 = getPhysicalIndex( number2 );
00327 
00328         /* consistency check */
00329         if ( ( index1 < 0 ) || ( index2 < 0 ) )
00330                 return THROWERROR( RET_INDEXLIST_CORRUPTED );
00331 
00332         int tmp = number[index1];
00333         number[index1] = number[index2];
00334         number[index2] = tmp;
00335 
00336         return SUCCESSFUL_RETURN;
00337 }
00338 
00339 
00340 /*
00341  *      end of file
00342  */


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