Indexlist.c
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-2015 by Hans Joachim Ferreau, Andreas Potschka,
6  * Christian Kirches et al. All rights reserved.
7  *
8  * qpOASES is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU Lesser General Public
10  * License as published by the Free Software Foundation; either
11  * version 2.1 of the License, or (at your option) any later version.
12  *
13  * qpOASES is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
16  * See the GNU Lesser General Public License for more details.
17  *
18  * You should have received a copy of the GNU Lesser General Public
19  * License along with qpOASES; if not, write to the Free Software
20  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
21  *
22  */
23 
24 
36 #include <qpOASES_e/Indexlist.h>
37 
38 
40 
41 
42 /*****************************************************************************
43  * P U B L I C *
44  *****************************************************************************/
45 
46 /*
47  * I n d e x l i s t
48  */
49 void IndexlistCON( Indexlist* _THIS,
50  int n
51  )
52 {
53  Indexlist_init( _THIS,n );
54 }
55 
56 
57 /*
58  * c o p y
59  */
60 void IndexlistCPY( Indexlist* FROM,
61  Indexlist* TO
62  )
63 {
64  int i;
65 
66  if ( FROM != TO )
67  {
68  TO->length = FROM->length;
69  TO->physicallength = FROM->physicallength;
70 
71  if ( FROM->number != 0 )
72  {
73  for( i=0; i<TO->physicallength; ++i )
74  TO->number[i] = FROM->number[i];
75  for( i=0; i<TO->physicallength; ++i )
76  TO->iSort[i] = FROM->iSort[i];
77  }
78  }
79 }
80 
81 
82 
83 /*
84  * i n i t
85  */
87  int n
88  )
89 {
90  if ( n < 0 )
92 
93  _THIS->length = 0;
94  _THIS->physicallength = n;
95 
96  assert( n <= NVCMAX );
97 
98  return SUCCESSFUL_RETURN;
99 }
100 
101 
102 /*
103  * g e t N u m b e r A r r a y
104  */
105 returnValue Indexlist_getNumberArray( Indexlist* _THIS, int** const numberarray )
106 {
107  if (numberarray == 0)
109 
110  *numberarray = _THIS->number;
111  return SUCCESSFUL_RETURN;
112 }
113 
114 
115 /*
116  * g e t I S o r t A r r a y
117  */
118 returnValue Indexlist_getISortArray( Indexlist* _THIS, int** const iSortArray )
119 {
120  *iSortArray = _THIS->iSort;
121  return SUCCESSFUL_RETURN;
122 }
123 
124 
125 
126 /*
127  * g e t I n d e x
128  */
129 int Indexlist_getIndex( Indexlist* _THIS, int givennumber )
130 {
131  int myIndex = Indexlist_findInsert(_THIS,givennumber);
132  return _THIS->number[_THIS->iSort[myIndex]] == givennumber ? _THIS->iSort[myIndex] : -1;
133 }
134 
135 
136 /*
137  * a d d N u m b e r
138  */
139 returnValue Indexlist_addNumber( Indexlist* _THIS, int addnumber )
140 {
141  int i, j;
142 
143  if ( _THIS->length >= _THIS->physicallength )
145 
146  _THIS->number[_THIS->length] = addnumber;
147  j = Indexlist_findInsert(_THIS,addnumber);
148  for (i = _THIS->length; i > j+1; i--)
149  _THIS->iSort[i] = _THIS->iSort[i-1];
150  _THIS->iSort[j+1] = _THIS->length;
151  ++(_THIS->length);
152 
153  return SUCCESSFUL_RETURN;
154 }
155 
156 
157 /*
158  * r e m o v e N u m b e r
159  */
160 returnValue Indexlist_removeNumber( Indexlist* _THIS, int removenumber )
161 {
162  int i;
163  int idx = Indexlist_findInsert( _THIS,removenumber );
164  int iSidx = _THIS->iSort[idx];
165 
166  /* nothing to be done if number is not contained in index set */
167  if ( _THIS->number[iSidx] != removenumber )
168  return SUCCESSFUL_RETURN;
169 
170  /* update sorted indices iSort first */
171  for (i = 0; i < _THIS->length; i++)
172  if (_THIS->iSort[i] > iSidx) _THIS->iSort[i]--;
173  for (i = idx+1; i < _THIS->length; i++)
174  _THIS->iSort[i-1] = _THIS->iSort[i];
175 
176  /* remove from numbers list */
177  for( i=iSidx; i<_THIS->length-1; ++i )
178  _THIS->number[i] = _THIS->number[i+1];
179  _THIS->number[_THIS->length-1] = -1;
180 
181  --(_THIS->length);
182 
183  return SUCCESSFUL_RETURN;
184 }
185 
186 
187 /*
188  * s w a p N u m b e r s
189  */
190 returnValue Indexlist_swapNumbers( Indexlist* _THIS, int number1, int number2 )
191 {
192  int index1 = Indexlist_findInsert( _THIS,number1 );
193  int index2 = Indexlist_findInsert( _THIS,number2 );
194  int tmp;
195 
196  /* consistency check */
197  if ( ( _THIS->number[_THIS->iSort[index1]] != number1 ) || ( _THIS->number[_THIS->iSort[index2]] != number2 ) )
199 
200  /* swap numbers */
201  tmp = _THIS->number[_THIS->iSort[index1]];
202  _THIS->number[_THIS->iSort[index1]] = _THIS->number[_THIS->iSort[index2]];
203  _THIS->number[_THIS->iSort[index2]] = tmp;
204  /* swap sorting indices */
205  tmp = _THIS->iSort[index1];
206  _THIS->iSort[index1] = _THIS->iSort[index2];
207  _THIS->iSort[index2] = tmp;
208 
209  return SUCCESSFUL_RETURN;
210 }
211 
212 
213 
214 /*****************************************************************************
215  * P R O T E C T E D *
216  *****************************************************************************/
217 
218 
219 int Indexlist_findInsert( Indexlist* _THIS, int i )
220 {
221  int fst = 0, lst = _THIS->length-1, mid;
222 
223  /* quick check if index can be appended */
224  if (_THIS->length == 0 || i < _THIS->number[_THIS->iSort[0]]) return -1;
225  if (i >= _THIS->number[_THIS->iSort[_THIS->length-1]]) return _THIS->length-1;
226 
227  /* otherwise, perform bisection search */
228  while (fst < lst - 1)
229  {
230  mid = (fst + lst) / 2;
231  if (i >= _THIS->number[_THIS->iSort[mid]]) fst = mid;
232  else lst = mid;
233  }
234 
235  return fst;
236 }
237 
239 
240 
241 /*
242  * end of file
243  */
int Indexlist_getIndex(Indexlist *_THIS, int givennumber)
Definition: Indexlist.c:129
returnValue Indexlist_getISortArray(Indexlist *_THIS, int **const iSortArray)
Definition: Indexlist.c:118
Allows to pass back messages to the calling function.
returnValue Indexlist_addNumber(Indexlist *_THIS, int addnumber)
Definition: Indexlist.c:139
void IndexlistCPY(Indexlist *FROM, Indexlist *TO)
Definition: Indexlist.c:60
returnValue Indexlist_removeNumber(Indexlist *_THIS, int removenumber)
Definition: Indexlist.c:160
returnValue Indexlist_getNumberArray(Indexlist *_THIS, int **const numberarray)
Definition: Indexlist.c:105
returnValue Indexlist_init(Indexlist *_THIS, int n)
Definition: Indexlist.c:86
#define assert(ignore)
BEGIN_NAMESPACE_QPOASES void IndexlistCON(Indexlist *_THIS, int n)
Definition: Indexlist.c:49
int Indexlist_findInsert(Indexlist *_THIS, int i)
Definition: Indexlist.c:219
returnValue Indexlist_swapNumbers(Indexlist *_THIS, int number1, int number2)
Definition: Indexlist.c:190


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