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
25 /**
26 * \file src/SubjectTo.cpp
27 * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
28 * \version 3.2
29 * \date 2007-2015
30 *
31 * Implementation of the SubjectTo class designed to manage working sets of
32 * constraints and bounds within a QProblem.
33 */
34
35
36 #include <qpOASES/SubjectTo.hpp>
37
38
39 BEGIN_NAMESPACE_QPOASES
40
41
42 /*****************************************************************************
43 * P U B L I C *
44 *****************************************************************************/
45
46
47 /*
48 * S u b j e c t T o
49 */
SubjectTo()50 SubjectTo::SubjectTo( )
51 {
52 type = 0;
53 status = 0;
54
55 init( );
56 }
57
58
59 /*
60 * S u b j e c t T o
61 */
SubjectTo(int_t _n)62 SubjectTo::SubjectTo( int_t _n )
63 {
64 type = 0;
65 status = 0;
66
67 init( _n );
68 }
69
70
71 /*
72 * S u b j e c t T o
73 */
SubjectTo(const SubjectTo & rhs)74 SubjectTo::SubjectTo( const SubjectTo& rhs )
75 {
76 copy( rhs );
77 }
78
79
80 /*
81 * ~ S u b j e c t T o
82 */
~SubjectTo()83 SubjectTo::~SubjectTo( )
84 {
85 clear( );
86 }
87
88
89 /*
90 * o p e r a t o r =
91 */
operator =(const SubjectTo & rhs)92 SubjectTo& SubjectTo::operator=( const SubjectTo& rhs )
93 {
94 if ( this != &rhs )
95 {
96 clear( );
97 copy( rhs );
98 }
99
100 return *this;
101 }
102
103
104 /*
105 * i n i t
106 */
init(int_t _n)107 returnValue SubjectTo::init( int_t _n
108 )
109 {
110 int_t i;
111
112 if ( _n < 0 )
113 return THROWERROR( RET_INVALID_ARGUMENTS );
114
115 clear( );
116
117 n = _n;
118 noLower = BT_TRUE;
119 noUpper = BT_TRUE;
120
121 if ( n > 0 )
122 {
123 type = new SubjectToType[n];
124 status = new SubjectToStatus[n];
125
126 for( i=0; i<n; ++i )
127 {
128 type[i] = ST_UNKNOWN;
129 status[i] = ST_UNDEFINED;
130 }
131 }
132
133 return SUCCESSFUL_RETURN;
134 }
135
136
137
138 /*****************************************************************************
139 * P R O T E C T E D *
140 *****************************************************************************/
141
142 /*
143 * c l e a r
144 */
clear()145 returnValue SubjectTo::clear( )
146 {
147 if ( type != 0 )
148 {
149 delete[] type;
150 type = 0;
151 }
152
153 if ( status != 0 )
154 {
155 delete[] status;
156 status = 0;
157 }
158
159 return SUCCESSFUL_RETURN;
160 }
161
162
163 /*
164 * c o p y
165 */
copy(const SubjectTo & rhs)166 returnValue SubjectTo::copy( const SubjectTo& rhs
167 )
168 {
169 int_t i;
170
171 n = rhs.n;
172 noLower = rhs.noLower;
173 noUpper = rhs.noUpper;
174
175 if ( rhs.n != 0 )
176 {
177 type = new SubjectToType[n];
178 status = new SubjectToStatus[n];
179
180 for( i=0; i<n; ++i )
181 {
182 type[i] = rhs.type[i];
183 status[i] = rhs.status[i];
184 }
185 }
186 else
187 {
188 type = 0;
189 status = 0;
190 }
191
192 return SUCCESSFUL_RETURN;
193 }
194
195
196 /*
197 * a d d I n d e x
198 */
addIndex(Indexlist * const indexlist,int_t newnumber,SubjectToStatus newstatus)199 returnValue SubjectTo::addIndex( Indexlist* const indexlist,
200 int_t newnumber, SubjectToStatus newstatus
201 )
202 {
203 if ( status != 0 )
204 {
205 /* consistency check */
206 if ( status[newnumber] == newstatus )
207 return THROWERROR( RET_INDEX_ALREADY_OF_DESIRED_STATUS );
208
209 status[newnumber] = newstatus;
210 }
211 else
212 return THROWERROR( RET_ADDINDEX_FAILED );
213
214 if ( indexlist != 0 )
215 {
216 if ( indexlist->addNumber( newnumber ) == RET_INDEXLIST_EXCEEDS_MAX_LENGTH )
217 return THROWERROR( RET_ADDINDEX_FAILED );
218 }
219 else
220 return THROWERROR( RET_INVALID_ARGUMENTS );
221
222 return SUCCESSFUL_RETURN;
223 }
224
225
226 /*
227 * r e m o v e I n d e x
228 */
removeIndex(Indexlist * const indexlist,int_t removenumber)229 returnValue SubjectTo::removeIndex( Indexlist* const indexlist,
230 int_t removenumber
231 )
232 {
233 if ( status != 0 )
234 status[removenumber] = ST_UNDEFINED;
235 else
236 return THROWERROR( RET_REMOVEINDEX_FAILED );
237
238 if ( indexlist != 0 )
239 {
240 if ( indexlist->removeNumber( removenumber ) != SUCCESSFUL_RETURN )
241 return THROWERROR( RET_REMOVEINDEX_FAILED );
242 }
243 else
244 return THROWERROR( RET_INVALID_ARGUMENTS );
245
246 return SUCCESSFUL_RETURN;
247 }
248
249
250 /*
251 * s w a p I n d e x
252 */
swapIndex(Indexlist * const indexlist,int_t number1,int_t number2)253 returnValue SubjectTo::swapIndex( Indexlist* const indexlist,
254 int_t number1, int_t number2
255 )
256 {
257 /* consistency checks */
258 if ( status != 0 )
259 {
260 if ( status[number1] != status[number2] )
261 return THROWERROR( RET_SWAPINDEX_FAILED );
262 }
263 else
264 return THROWERROR( RET_SWAPINDEX_FAILED );
265
266 if ( number1 == number2 )
267 {
268 THROWWARNING( RET_NOTHING_TO_DO );
269 return SUCCESSFUL_RETURN;
270 }
271
272 if ( indexlist != 0 )
273 {
274 if ( indexlist->swapNumbers( number1,number2 ) != SUCCESSFUL_RETURN )
275 return THROWERROR( RET_SWAPINDEX_FAILED );
276 }
277 else
278 return THROWERROR( RET_INVALID_ARGUMENTS );
279
280 return SUCCESSFUL_RETURN;
281 }
282
283
284 END_NAMESPACE_QPOASES
285
286
287 /*
288 * end of file
289 */
290