1 /*
2 * This file is part of qpOASES.
3 *
4 * qpOASES -- An Implementation of the Online Active Set Strategy.
5 * Copyright (C) 2007-2017 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
25 /**
26 * \file src/Bounds.cpp
27 * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
28 * \version 3.2
29 * \date 2007-2017
30 *
31 * Implementation of the Bounds class designed to manage working sets of
32 * bounds within a QProblem.
33 */
34
35
36 #include <qpOASES/Bounds.hpp>
37
38
39 BEGIN_NAMESPACE_QPOASES
40
41
42 /*****************************************************************************
43 * P U B L I C *
44 *****************************************************************************/
45
46
47 /*
48 * B o u n d s
49 */
Bounds()50 Bounds::Bounds( ) : SubjectTo( )
51 {
52 }
53
54
55 /*
56 * B o u n d s
57 */
Bounds(int_t _n)58 Bounds::Bounds( int_t _n ) : SubjectTo( _n )
59 {
60 init( _n );
61 }
62
63
64 /*
65 * B o u n d s
66 */
Bounds(const Bounds & rhs)67 Bounds::Bounds( const Bounds& rhs ) : SubjectTo( rhs )
68 {
69 copy( rhs );
70 }
71
72
73 /*
74 * ~ B o u n d s
75 */
~Bounds()76 Bounds::~Bounds( )
77 {
78 clear( );
79 }
80
81
82 /*
83 * o p e r a t o r =
84 */
operator =(const Bounds & rhs)85 Bounds& Bounds::operator=( const Bounds& rhs )
86 {
87 if ( this != &rhs )
88 {
89 clear( );
90 SubjectTo::operator=( rhs );
91 copy( rhs );
92 }
93
94 return *this;
95 }
96
97
98
99 /*
100 * i n i t
101 */
init(int_t _n)102 returnValue Bounds::init( int_t _n
103 )
104 {
105 if ( _n < 0 )
106 return THROWERROR( RET_INVALID_ARGUMENTS );
107
108 clear( );
109
110 if ( _n >= 0 )
111 {
112 freee.init( _n );
113 fixed.init( _n );
114 }
115
116 return SubjectTo::init( _n );
117 }
118
119
120
121 /*
122 * s e t u p B o u n d
123 */
setupBound(int_t number,SubjectToStatus _status)124 returnValue Bounds::setupBound( int_t number, SubjectToStatus _status
125 )
126 {
127 /* consistency check */
128 if ( ( number < 0 ) || ( number >= n ) )
129 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
130
131 /* Add bound index to respective index list. */
132 switch ( _status )
133 {
134 case ST_INACTIVE:
135 if ( this->addIndex( this->getFree( ),number,_status ) != SUCCESSFUL_RETURN )
136 return THROWERROR( RET_SETUP_BOUND_FAILED );
137 break;
138
139 case ST_LOWER:
140 if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
141 return THROWERROR( RET_SETUP_BOUND_FAILED );
142 break;
143
144 case ST_UPPER:
145 if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
146 return THROWERROR( RET_SETUP_BOUND_FAILED );
147 break;
148
149 default:
150 return THROWERROR( RET_INVALID_ARGUMENTS );
151 }
152
153 return SUCCESSFUL_RETURN;
154 }
155
156
157 /*
158 * s e t u p A l l F r e e
159 */
setupAllFree()160 returnValue Bounds::setupAllFree( )
161 {
162 return setupAll( ST_INACTIVE );
163 }
164
165
166 /*
167 * s e t u p A l l L o w e r
168 */
setupAllLower()169 returnValue Bounds::setupAllLower( )
170 {
171 return setupAll( ST_LOWER );
172 }
173
174
175 /*
176 * s e t u p A l l U p p e r
177 */
setupAllUpper()178 returnValue Bounds::setupAllUpper( )
179 {
180 return setupAll( ST_UPPER );
181 }
182
183
184 /*
185 * m o v e F i x e d T o F r e e
186 */
moveFixedToFree(int_t number)187 returnValue Bounds::moveFixedToFree( int_t number )
188 {
189 /* consistency check */
190 if ( ( number < 0 ) || ( number >= n ) )
191 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
192
193 /* Move index from indexlist of fixed variables to that of free ones. */
194 if ( this->removeIndex( this->getFixed( ),number ) != SUCCESSFUL_RETURN )
195 return THROWERROR( RET_MOVING_BOUND_FAILED );
196
197 if ( this->addIndex( this->getFree( ),number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
198 return THROWERROR( RET_MOVING_BOUND_FAILED );
199
200 return SUCCESSFUL_RETURN;
201 }
202
203
204 /*
205 * m o v e F r e e T o F i x e d
206 */
moveFreeToFixed(int_t number,SubjectToStatus _status)207 returnValue Bounds::moveFreeToFixed( int_t number, SubjectToStatus _status
208 )
209 {
210 /* consistency check */
211 if ( ( number < 0 ) || ( number >= n ) )
212 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
213
214 /* Move index from indexlist of free variables to that of fixed ones. */
215 if ( this->removeIndex( this->getFree( ),number ) != SUCCESSFUL_RETURN )
216 return THROWERROR( RET_MOVING_BOUND_FAILED );
217
218 if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
219 return THROWERROR( RET_MOVING_BOUND_FAILED );
220
221 return SUCCESSFUL_RETURN;
222 }
223
224
225 /*
226 * f l i p F i x e d
227 */
flipFixed(int_t number)228 returnValue Bounds::flipFixed( int_t number )
229 {
230 /* consistency check */
231 if ( ( number < 0 ) || ( number >= n ) )
232 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
233
234 if ( status != 0 )
235 switch (status[number])
236 {
237 case ST_LOWER: status[number] = ST_UPPER; break;
238 case ST_UPPER: status[number] = ST_LOWER; break;
239 default: return THROWERROR( RET_MOVING_BOUND_FAILED );
240 }
241
242 return SUCCESSFUL_RETURN;
243 }
244
245
246 /*
247 * s w a p F r e e
248 */
swapFree(int_t number1,int_t number2)249 returnValue Bounds::swapFree( int_t number1, int_t number2
250 )
251 {
252 /* consistency check */
253 if ( ( number1 < 0 ) || ( number1 >= n ) || ( number2 < 0 ) || ( number2 >= n ) )
254 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
255
256 /* Swap index within indexlist of free variables. */
257 return this->swapIndex( this->getFree( ),number1,number2 );
258 }
259
260
261 /*
262 * s h i f t
263 */
shift(int_t offset)264 returnValue Bounds::shift( int_t offset )
265 {
266 int_t i;
267
268 /* consistency check */
269 if ( ( offset == 0 ) || ( n <= 1 ) )
270 return SUCCESSFUL_RETURN;
271
272 if ( ( offset < 0 ) || ( offset > n/2 ) )
273 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
274
275 if ( ( n % offset ) != 0 )
276 return THROWERROR( RET_INVALID_ARGUMENTS );
277
278
279 /* 1) Shift types and status. */
280 for( i=0; i<n-offset; ++i )
281 {
282 setType( i,getType( i+offset ) );
283 setStatus( i,getStatus( i+offset ) );
284 }
285
286 /* 2) Construct shifted index lists of free and fixed variables. */
287 Indexlist shiftedFreee( n );
288 Indexlist shiftedFixed( n );
289
290 for( i=0; i<n; ++i )
291 {
292 switch ( getStatus( i ) )
293 {
294 case ST_INACTIVE:
295 if ( shiftedFreee.addNumber( i ) != SUCCESSFUL_RETURN )
296 return THROWERROR( RET_SHIFTING_FAILED );
297 break;
298
299 case ST_LOWER:
300 if ( shiftedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
301 return THROWERROR( RET_SHIFTING_FAILED );
302 break;
303
304 case ST_UPPER:
305 if ( shiftedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
306 return THROWERROR( RET_SHIFTING_FAILED );
307 break;
308
309 default:
310 return THROWERROR( RET_SHIFTING_FAILED );
311 }
312 }
313
314 /* 3) Assign shifted index list. */
315 freee = shiftedFreee;
316 fixed = shiftedFixed;
317
318 return SUCCESSFUL_RETURN;
319 }
320
321
322 /*
323 * r o t a t e
324 */
rotate(int_t offset)325 returnValue Bounds::rotate( int_t offset )
326 {
327 int_t i;
328
329 /* consistency check */
330 if ( ( offset == 0 ) || ( offset == n ) || ( n <= 1 ) )
331 return SUCCESSFUL_RETURN;
332
333 if ( ( offset < 0 ) || ( offset > n ) )
334 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
335
336
337 /* 1) Rotate types and status. */
338 SubjectToType* typeTmp = new SubjectToType[offset];
339 SubjectToStatus* statusTmp = new SubjectToStatus[offset];
340
341 for( i=0; i<offset; ++i )
342 {
343 typeTmp[i] = getType( i );
344 statusTmp[i] = getStatus( i );
345 }
346
347 for( i=0; i<n-offset; ++i )
348 {
349 setType( i,getType( i+offset ) );
350 setStatus( i,getStatus( i+offset ) );
351 }
352
353 for( i=n-offset; i<n; ++i )
354 {
355 setType( i,typeTmp[i-n+offset] );
356 setStatus( i,statusTmp[i-n+offset] );
357 }
358
359 delete[] statusTmp; delete[] typeTmp;
360
361 /* 2) Construct shifted index lists of free and fixed variables. */
362 Indexlist rotatedFreee( n );
363 Indexlist rotatedFixed( n );
364
365 for( i=0; i<n; ++i )
366 {
367 switch ( getStatus( i ) )
368 {
369 case ST_INACTIVE:
370 if ( rotatedFreee.addNumber( i ) != SUCCESSFUL_RETURN )
371 return THROWERROR( RET_ROTATING_FAILED );
372 break;
373
374 case ST_LOWER:
375 if ( rotatedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
376 return THROWERROR( RET_ROTATING_FAILED );
377 break;
378
379 case ST_UPPER:
380 if ( rotatedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
381 return THROWERROR( RET_ROTATING_FAILED );
382 break;
383
384 default:
385 return THROWERROR( RET_ROTATING_FAILED );
386 }
387 }
388
389 /* 3) Assign shifted index list. */
390 freee = rotatedFreee;
391 fixed = rotatedFixed;
392
393 return SUCCESSFUL_RETURN;
394 }
395
396
397 /*
398 * p r i n t
399 */
print()400 returnValue Bounds::print( )
401 {
402 if ( n == 0 )
403 return SUCCESSFUL_RETURN;
404
405 #ifndef __SUPPRESSANYOUTPUT__
406
407 char myPrintfString[MAX_STRING_LENGTH];
408
409 int_t nFR = getNFR( );
410 int_t nFX = getNFX( );
411
412 int_t* FR_idx;
413 getFree( )->getNumberArray( &FR_idx );
414
415 int_t* FX_idx;
416 getFixed( )->getNumberArray( &FX_idx );
417
418 snprintf( myPrintfString,MAX_STRING_LENGTH,"Bounds object comprising %d variables (%d free, %d fixed):\n",(int)n,(int)nFR,(int)nFX );
419 myPrintf( myPrintfString );
420
421 REFER_NAMESPACE_QPOASES print( FR_idx,nFR,"free " );
422 REFER_NAMESPACE_QPOASES print( FX_idx,nFX,"fixed" );
423
424 #endif /* __SUPPRESSANYOUTPUT__ */
425
426 return SUCCESSFUL_RETURN;
427 }
428
429
430
431 /*****************************************************************************
432 * P R O T E C T E D *
433 *****************************************************************************/
434
435 /*
436 * c l e a r
437 */
clear()438 returnValue Bounds::clear( )
439 {
440 return SUCCESSFUL_RETURN;
441 }
442
443
444 /*
445 * c o p y
446 */
copy(const Bounds & rhs)447 returnValue Bounds::copy( const Bounds& rhs
448 )
449 {
450 freee = rhs.freee;
451 fixed = rhs.fixed;
452
453 return SUCCESSFUL_RETURN;
454 }
455
456
457
458 /*
459 * s e t u p A l l
460 */
setupAll(SubjectToStatus _status)461 returnValue Bounds::setupAll( SubjectToStatus _status )
462 {
463 int_t i;
464
465 /* 1) Place unbounded variables at the beginning of the index list of free variables. */
466 for( i=0; i<n; ++i )
467 {
468 if ( getType( i ) == ST_UNBOUNDED )
469 {
470 if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
471 return THROWERROR( RET_SETUP_BOUND_FAILED );
472 }
473 }
474
475 /* 2) Add remaining (i.e. bounded but possibly free) variables to the index list of free variables. */
476 for( i=0; i<n; ++i )
477 {
478 if ( getType( i ) == ST_BOUNDED )
479 {
480 if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
481 return THROWERROR( RET_SETUP_BOUND_FAILED );
482 }
483 }
484
485 /* 3) Place implicitly fixed variables at the end of the index list of free variables. */
486 for( i=0; i<n; ++i )
487 {
488 if ( getType( i ) == ST_EQUALITY )
489 {
490 if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
491 return THROWERROR( RET_SETUP_BOUND_FAILED );
492 }
493 }
494
495 /* 4) Moreover, add all bounds of unknown type. */
496 for( i=0; i<n; ++i )
497 {
498 if ( getType( i ) == ST_UNKNOWN || getType( i ) == ST_DISABLED )
499 {
500 if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
501 return THROWERROR( RET_SETUP_BOUND_FAILED );
502 }
503 }
504
505 return SUCCESSFUL_RETURN;
506 }
507
508
509 END_NAMESPACE_QPOASES
510
511
512 /*
513 * end of file
514 */
515