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/Indexlist.hpp> 00037 00038 00039 BEGIN_NAMESPACE_QPOASES 00040 00041 00042 /***************************************************************************** 00043 * P U B L I C * 00044 *****************************************************************************/ 00045 00046 00047 /* 00048 * I n d e x l i s t 00049 */ 00050 Indexlist::Indexlist( ) 00051 { 00052 number = 0; 00053 iSort = 0; 00054 00055 init( ); 00056 } 00057 00058 00059 /* 00060 * I n d e x l i s t 00061 */ 00062 Indexlist::Indexlist( int n ) 00063 { 00064 number = 0; 00065 iSort = 0; 00066 00067 init( n ); 00068 } 00069 00070 00071 /* 00072 * I n d e x l i s t 00073 */ 00074 Indexlist::Indexlist( const Indexlist& rhs ) 00075 { 00076 copy( rhs ); 00077 } 00078 00079 00080 /* 00081 * ~ I n d e x l i s t 00082 */ 00083 Indexlist::~Indexlist( ) 00084 { 00085 clear( ); 00086 } 00087 00088 00089 /* 00090 * o p e r a t o r = 00091 */ 00092 Indexlist& Indexlist::operator=( const Indexlist& rhs ) 00093 { 00094 if ( this != &rhs ) 00095 { 00096 clear( ); 00097 copy( rhs ); 00098 } 00099 00100 return *this; 00101 } 00102 00103 00104 00105 /* 00106 * i n i t 00107 */ 00108 returnValue Indexlist::init( int n 00109 ) 00110 { 00111 if ( n < 0 ) 00112 return THROWERROR( RET_INVALID_ARGUMENTS ); 00113 00114 clear( ); 00115 00116 length = 0; 00117 physicallength = n; 00118 00119 if ( n > 0 ) 00120 { 00121 number = new int[n]; 00122 iSort = new int[n]; 00123 } 00124 00125 return SUCCESSFUL_RETURN; 00126 } 00127 00128 00129 /* 00130 * g e t N u m b e r A r r a y 00131 */ 00132 returnValue Indexlist::getNumberArray( int** const numberarray ) const 00133 { 00134 if (numberarray == 0) 00135 return THROWERROR( RET_INVALID_ARGUMENTS ); 00136 00137 *numberarray = number; 00138 00139 return SUCCESSFUL_RETURN; 00140 } 00141 00142 00143 /* 00144 * g e t I n d e x 00145 */ 00146 int Indexlist::getIndex( int givennumber ) const 00147 { 00148 int index = findInsert(givennumber); 00149 return number[iSort[index]] == givennumber ? iSort[index] : -1; 00150 } 00151 00152 00153 /* 00154 * a d d N u m b e r 00155 */ 00156 returnValue Indexlist::addNumber( int addnumber ) 00157 { 00158 if ( length >= physicallength ) 00159 return THROWERROR( RET_INDEXLIST_EXCEEDS_MAX_LENGTH ); 00160 00161 int i, j; 00162 number[length] = addnumber; 00163 j = findInsert(addnumber); 00164 for (i = length; i > j+1; i--) 00165 iSort[i] = iSort[i-1]; 00166 iSort[j+1] = length; 00167 ++length; 00168 00169 return SUCCESSFUL_RETURN; 00170 } 00171 00172 00173 /* 00174 * r e m o v e N u m b e r 00175 */ 00176 returnValue Indexlist::removeNumber( int removenumber ) 00177 { 00178 int i; 00179 int idx = findInsert( removenumber ); 00180 int iSidx = iSort[idx]; 00181 00182 /* nothing to be done if number is not contained in index set */ 00183 if ( number[iSidx] != removenumber ) 00184 return SUCCESSFUL_RETURN; 00185 00186 /* update sorted indices iSort first */ 00187 for (i = 0; i < length; i++) 00188 if (iSort[i] > iSidx) iSort[i]--; 00189 for (i = idx+1; i < length; i++) 00190 iSort[i-1] = iSort[i]; 00191 00192 /* remove from numbers list */ 00193 for( i=iSidx; i<length-1; ++i ) 00194 number[i] = number[i+1]; 00195 number[length-1] = -1; 00196 00197 --length; 00198 00199 return SUCCESSFUL_RETURN; 00200 } 00201 00202 00203 /* 00204 * s w a p N u m b e r s 00205 */ 00206 returnValue Indexlist::swapNumbers( int number1, int number2 ) 00207 { 00208 int index1 = findInsert( number1 ); 00209 int index2 = findInsert( number2 ); 00210 00211 /* consistency check */ 00212 if ( ( number[iSort[index1]] != number1 ) || ( number[iSort[index2]] != number2 ) ) 00213 return THROWERROR( RET_INDEXLIST_CORRUPTED ); 00214 00215 int tmp; 00216 /* swap numbers */ 00217 tmp = number[iSort[index1]]; 00218 number[iSort[index1]] = number[iSort[index2]]; 00219 number[iSort[index2]] = tmp; 00220 /* swap sorting indices */ 00221 tmp = iSort[index1]; 00222 iSort[index1] = iSort[index2]; 00223 iSort[index2] = tmp; 00224 00225 return SUCCESSFUL_RETURN; 00226 } 00227 00228 00229 00230 /***************************************************************************** 00231 * P R O T E C T E D * 00232 *****************************************************************************/ 00233 00234 /* 00235 * c l e a r 00236 */ 00237 returnValue Indexlist::clear( ) 00238 { 00239 if ( iSort != 0 ) 00240 { 00241 delete[] iSort; 00242 iSort = 0; 00243 } 00244 00245 if ( number != 0 ) 00246 { 00247 delete[] number; 00248 number = 0; 00249 } 00250 00251 return SUCCESSFUL_RETURN; 00252 } 00253 00254 00255 /* 00256 * c o p y 00257 */ 00258 returnValue Indexlist::copy( const Indexlist& rhs 00259 ) 00260 { 00261 int i; 00262 00263 length = rhs.length; 00264 physicallength = rhs.physicallength; 00265 00266 if ( rhs.number != 0 ) 00267 { 00268 number = new int[physicallength]; 00269 for( i=0; i<physicallength; ++i ) 00270 number[i] = rhs.number[i]; 00271 iSort = new int[physicallength]; 00272 for( i=0; i<physicallength; ++i ) 00273 iSort[i] = rhs.iSort[i]; 00274 } 00275 else 00276 { 00277 number = 0; 00278 iSort = 0; 00279 } 00280 00281 return SUCCESSFUL_RETURN; 00282 } 00283 00284 int Indexlist::findInsert(int i) const 00285 { 00286 /* quick check if index can be appended */ 00287 if (length == 0 || i < number[iSort[0]]) return -1; 00288 if (i >= number[iSort[length-1]]) return length-1; 00289 00290 /* otherwise, perform bisection search */ 00291 int fst = 0, lst = length-1, mid; 00292 00293 while (fst < lst - 1) 00294 { 00295 mid = (fst + lst) / 2; 00296 if (i >= number[iSort[mid]]) fst = mid; 00297 else lst = mid; 00298 } 00299 00300 return fst; 00301 } 00302 00303 END_NAMESPACE_QPOASES 00304 00305 00306 /* 00307 * end of file 00308 */