external_packages/qpoases/SRC/Indexlist.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of qpOASES.
3  *
4  * qpOASES -- An Implementation of the Online Active Set Strategy.
5  * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
6  *
7  * qpOASES is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public
9  * License as published by the Free Software Foundation; either
10  * version 2.1 of the License, or (at your option) any later version.
11  *
12  * qpOASES is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15  * Lesser General Public License for more details.
16  *
17  * You should have received a copy of the GNU Lesser General Public
18  * License along with qpOASES; if not, write to the Free Software
19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20  *
21  */
22 
23 
35 #include <Indexlist.hpp>
36 
37 
38 /*****************************************************************************
39  * P U B L I C *
40  *****************************************************************************/
41 
42 
43 /*
44  * I n d e x l i s t
45  */
46 Indexlist::Indexlist( ) : length( 0 ),
47  first( -1 ),
48  last( -1 ),
49  lastusedindex( -1 ),
50  physicallength( INDEXLISTFACTOR*(NVMAX+NCMAX) )
51 {
52  int i;
53 
54  for( i=0; i<physicallength; ++i )
55  {
56  number[i] = -1;
57  next[i] = -1;
58  previous[i] = -1;
59  }
60 }
61 
62 
63 /*
64  * I n d e x l i s t
65  */
67  first( rhs.first ),
68  last( rhs.last ),
71 {
72  int i;
73 
74  for( i=0; i<physicallength; ++i )
75  {
76  number[i] = rhs.number[i];
77  next[i] = rhs.next[i];
78  previous[i] = rhs.previous[i];
79  }
80 }
81 
82 
83 /*
84  * ~ I n d e x l i s t
85  */
87 {
88 }
89 
90 
91 /*
92  * o p e r a t o r =
93  */
95 {
96  int i;
97 
98  if ( this != &rhs )
99  {
100  length = rhs.length;
101  first = rhs.first;
102  last = rhs.last;
105 
106  for( i=0; i<physicallength; ++i )
107  {
108  number[i] = rhs.number[i];
109  next[i] = rhs.next[i];
110  previous[i] = rhs.previous[i];
111  }
112  }
113 
114  return *this;
115 }
116 
117 
118 /*
119  * i n i t
120  */
122 {
123  int i;
124 
125  length = 0;
126  first = -1;
127  last = -1;
128  lastusedindex = -1;
130 
131  for( i=0; i<physicallength; ++i )
132  {
133  number[i] = -1;
134  next[i] = -1;
135  previous[i] = -1;
136  }
137 
138  return SUCCESSFUL_RETURN;
139 }
140 
141 
142 /*
143  * g e t N u m b e r A r r a y
144  */
145 returnValue Indexlist::getNumberArray( int* const numberarray ) const
146 {
147  int i;
148  int n = first;
149 
150  /* Run trough indexlist and store numbers in numberarray. */
151  for( i=0; i<length; ++i )
152  {
153  if ( ( n >= 0 ) && ( number[n] >= 0 ) )
154  numberarray[i] = number[n];
155  else
157 
158  n = next[n];
159  }
160 
161  return SUCCESSFUL_RETURN;
162 }
163 
164 
165 /*
166  * g e t I n d e x
167  */
168 int Indexlist::getIndex( int givennumber ) const
169 {
170  int i;
171  int n = first;
172  int index = -1; /* return -1 by default */
173 
174  /* Run trough indexlist until number is found, if so return it index. */
175  for ( i=0; i<length; ++i )
176  {
177  if ( number[n] == givennumber )
178  {
179  index = i;
180  break;
181  }
182 
183  n = next[n];
184  }
185 
186  return index;
187 }
188 
189 
190 /*
191  * g e t P h y s i c a l I n d e x
192  */
193 int Indexlist::getPhysicalIndex( int givennumber ) const
194 {
195  int i;
196  int n = first;
197  int index = -1; /* return -1 by default */
198 
199  /* Run trough indexlist until number is found, if so return it physicalindex. */
200  for ( i=0; i<length; ++i )
201  {
202  if ( number[n] == givennumber )
203  {
204  index = n;
205  break;
206  }
207 
208  n = next[n];
209  }
210 
211  return index;
212 }
213 
214 
215 /*
216  * a d d N u m b e r
217  */
218 returnValue Indexlist::addNumber( int addnumber )
219 {
220  int i;
221 
222  if ( lastusedindex+1 < physicallength )
223  {
224  /* If there is enough storage, add number to indexlist. */
225  ++lastusedindex;
226  number[lastusedindex] = addnumber;
227  next[lastusedindex] = 0;
228 
229  if ( length == 0 )
230  {
232  previous[lastusedindex] = 0;
233  }
234  else
235  {
238  }
239 
241  ++length;
242 
243  return SUCCESSFUL_RETURN;
244  }
245  else
246  {
247  /* Rearrangement of index list necessary! */
248  if ( length == physicallength )
250  else
251  {
252  int numberArray[NVMAX+NCMAX];
253  getNumberArray( numberArray );
254 
255  /* copy existing elements */
256  for ( i=0; i<length; ++i )
257  {
258  number[i] = numberArray[i];
259  next[i] = i+1;
260  previous[i] = i-1;
261  }
262 
263  /* add new number at end of list */
264  number[length] = addnumber;
265  next[length] = -1;
266  previous[length] = length-1;
267 
268  /* and set remaining entries to empty */
269  for ( i=length+1; i<physicallength; ++i )
270  {
271  number[i] = -1;
272  next[i] = -1;
273  previous[i] = -1;
274  }
275 
276  first = 0;
277  last = length;
279  ++length;
280 
282  }
283  }
284 }
285 
286 
287 /*
288  * r e m o v e N u m b e r
289  */
290 returnValue Indexlist::removeNumber( int removenumber )
291 {
292  int i = getPhysicalIndex( removenumber );
293 
294  /* nothing to be done if number is not contained in index set */
295  if ( i < 0 )
296  return SUCCESSFUL_RETURN;
297 
298  int p = previous[i];
299  int n = next[i];
300 
301  if ( i == last )
302  last = p;
303  else
304  previous[n] = p;
305 
306  if ( i == first )
307  first = n;
308  else
309  next[p] = n;
310 
311  number[i] = -1;
312  next[i] = -1;
313  previous[i] = -1;
314  --length;
315 
316  return SUCCESSFUL_RETURN;
317 }
318 
319 
320 /*
321  * s w a p N u m b e r s
322  */
323 returnValue Indexlist::swapNumbers( int number1, int number2 )
324 {
325  int index1 = getPhysicalIndex( number1 );
326  int index2 = getPhysicalIndex( number2 );
327 
328  /* consistency check */
329  if ( ( index1 < 0 ) || ( index2 < 0 ) )
331 
332  int tmp = number[index1];
333  number[index1] = number[index2];
334  number[index2] = tmp;
335 
336  return SUCCESSFUL_RETURN;
337 }
338 
339 
340 /*
341  * end of file
342  */
Allows to pass back messages to the calling function.
void rhs(const real_t *x, real_t *f)


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:41