QProblem.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/QProblem.h>
37 #include <qpOASES_e/QProblemB.h>
38 
39 
40 
41 BEGIN_NAMESPACE_QPOASES
42 
43 
44 /*****************************************************************************
45  * P U B L I C *
46  *****************************************************************************/
47 
48 /*
49  * Q P r o b l e m
50  */
51 void QProblemCON( QProblem* _THIS,
52  int _nV, int _nC, HessianType _hessianType )
53 {
54  int i;
55 
56  #ifdef __CODE_GENERATION__
57  Options_setToFast( &(_THIS->options) );
58  #else
59  Options_setToDefault( &(_THIS->options) );
60  #endif /* __CODE_GENERATION__ */
61 
62  /* print copyright notice */
63  if (_THIS->options.printLevel != PL_NONE)
64  qpOASES_printCopyrightNotice( );
65 
66  /* consistency checks */
67  if ( ( _nV <= 0 ) || ( _nV > NVMAX ) )
68  {
69  _nV = 1;
70  THROWERROR( RET_INVALID_ARGUMENTS );
71  assert( 1 == 0 );
72  }
73 
74  if ( ( _nC < 0 ) || ( _nC > NCMAX ) )
75  {
76  _nC = 0;
77  THROWERROR( RET_INVALID_ARGUMENTS );
78  assert( 1 == 0 );
79  }
80 
81  /* reset global message handler */
82  MessageHandling_reset( qpOASES_getGlobalMessageHandler() );
83 
84  _THIS->H = &(_THIS->HH);
85 
86  for( i=0; i<_nV; ++i ) _THIS->g[i] = 0.0;
87  for( i=0; i<_nV; ++i ) _THIS->lb[i] = 0.0;
88  for( i=0; i<_nV; ++i ) _THIS->ub[i] = 0.0;
89 
90  for( i=0; i<_nV; ++i ) _THIS->x[i] = 0.0;
91  for( i=0; i<_nV+_nC; ++i ) _THIS->y[i] = 0.0;
92 
93  Bounds_init( &(_THIS->bounds),_nV );
94 
95  _THIS->haveCholesky = BT_FALSE;
96 
97  _THIS->tau = 0.0;
98 
99  _THIS->hessianType = _hessianType;
100  _THIS->regVal = 0.0;
101 
102  _THIS->infeasible = BT_FALSE;
103  _THIS->unbounded = BT_FALSE;
104 
105  _THIS->status = QPS_NOTINITIALISED;
106 
107  _THIS->count = 0;
108 
109  _THIS->ramp0 = _THIS->options.initialRamping;
110  _THIS->ramp1 = _THIS->options.finalRamping;
111  _THIS->rampOffset = 0;
112 
113  QProblem_setPrintLevel( _THIS,_THIS->options.printLevel );
114 
115  _THIS->A = &(_THIS->AA);
116 
117  for( i=0; i<_nC; ++i ) _THIS->lbA[i] = 0.0;
118  for( i=0; i<_nC; ++i ) _THIS->ubA[i] = 0.0;
119 
120  Constraints_init( &(_THIS->constraints),_nC );
121 
122  _THIS->sizeT = qpOASES_getMinI( _nV,_nC );
123 
124  _THIS->constraintProduct = 0;
125 }
126 
127 
128 
129 /*
130  * c o p y
131  */
132 void QProblemCPY( QProblem* FROM,
133  QProblem* TO
134  )
135 {
136  unsigned int _nV = (unsigned int)QProblem_getNV( FROM );
137  unsigned int _nC = (unsigned int)QProblem_getNC( FROM );
138 
139  TO->bounds = FROM->bounds;
140 
141  TO->HH = FROM->HH;
142  TO->H = &(TO->HH);
143 
144  QProblem_setG( TO,FROM->g );
145  QProblem_setLB( TO,FROM->lb );
146  QProblem_setUB( TO,FROM->ub );
147 
148  memcpy( TO->R,FROM->R,NVMAX*NVMAX*sizeof(real_t) );
149 
150  TO->haveCholesky = FROM->haveCholesky;
151 
152  memcpy( TO->x,FROM->x,_nV*sizeof(real_t) );
153 
154  TO->tau = FROM->tau;
155 
156  TO->hessianType = FROM->hessianType;
157  TO->regVal = FROM->regVal;
158 
159  TO->infeasible = FROM->infeasible;
160  TO->unbounded = FROM->unbounded;
161 
162  TO->status = FROM->status;
163 
164  TO->count = FROM->count;
165 
166  TO->ramp0 = FROM->ramp0;
167  TO->ramp1 = FROM->ramp1;
168 
169  OptionsCPY( &(FROM->options),&(TO->options) );
170  QProblem_setPrintLevel( TO,TO->options.printLevel );
171 
172  ConstraintsCPY( &(FROM->constraints),&(TO->constraints) );
173 
174  TO->AA = FROM->AA;
175  TO->A = &(TO->AA);
176 
177  QProblem_setLBA( TO,FROM->lbA );
178  QProblem_setUBA( TO,FROM->ubA );
179 
180  memcpy( TO->y,FROM->y,(_nV+_nC)*sizeof(real_t) );
181 
182  TO->sizeT = FROM->sizeT;
183 
184  memcpy( TO->T,FROM->T,NVCMIN*NVCMIN*sizeof(real_t) );
185  memcpy( TO->Q,FROM->Q,NVMAX*NVMAX*sizeof(real_t) );
186 
187  memcpy( TO->Ax,FROM->Ax,_nC*sizeof(real_t) );
188  memcpy( TO->Ax_l,FROM->Ax_l,_nC*sizeof(real_t) );
189  memcpy( TO->Ax_u,FROM->Ax_u,_nC*sizeof(real_t) );
190 
191  if ( FROM->constraintProduct != 0 )
192  TO->constraintProduct = FROM->constraintProduct;
193  else
194  TO->constraintProduct = 0;
195 }
196 
197 
198 
199 /*
200  * r e s e t
201  */
202 returnValue QProblem_reset( QProblem* _THIS )
203 {
204  int i;
205  int nV = QProblem_getNV( _THIS );
206  int nC = QProblem_getNC( _THIS );
207 
208  if ( nV == 0 )
209  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
210 
211 
212  /* 1) Reset bounds. */
213  Bounds_init( &(_THIS->bounds),nV );
214 
215  /* 2) Reset Cholesky decomposition. */
216  for( i=0; i<NVMAX*NVMAX; ++i )
217  _THIS->R[i] = 0.0;
218 
219  _THIS->haveCholesky = BT_FALSE;
220 
221  /* 3) Reset steplength and status flags. */
222  _THIS->tau = 0.0;
223 
224  _THIS->hessianType = HST_UNKNOWN;
225  _THIS->regVal = 0.0;
226 
227  _THIS->infeasible = BT_FALSE;
228  _THIS->unbounded = BT_FALSE;
229 
230  _THIS->status = QPS_NOTINITIALISED;
231 
232  _THIS->ramp0 = _THIS->options.initialRamping;
233  _THIS->ramp1 = _THIS->options.finalRamping;
234  _THIS->rampOffset = 0;
235 
236  /* 2) Reset constraints. */
237  Constraints_init( &(_THIS->constraints),nC );
238 
239  /* 3) Reset TQ factorisation. */
240  for( i=0; i<NVCMIN*NVCMIN; ++i )
241  _THIS->T[i] = 0.0;
242 
243  for( i=0; i<NVMAX*NVMAX; ++i )
244  _THIS->Q[i] = 0.0;
245 
246  /* 4) Reset constraint product pointer. */
247  _THIS->constraintProduct = 0;
248 
249  return SUCCESSFUL_RETURN;
250 }
251 
252 
253 /*
254  * i n i t
255  */
256 returnValue QProblem_initM( QProblem* _THIS, DenseMatrix *_H, const real_t* const _g, DenseMatrix *_A,
257  const real_t* const _lb, const real_t* const _ub,
258  const real_t* const _lbA, const real_t* const _ubA,
259  int* nWSR, real_t* const cputime
260  )
261 {
262  /*int nV,nC;*/
263 
264  if ( QProblem_getNV( _THIS ) == 0 )
265  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
266 
267  /* 1) Consistency check. */
268  if ( QProblem_isInitialised( _THIS ) == BT_TRUE )
269  {
270  THROWWARNING( RET_QP_ALREADY_INITIALISED );
271  QProblem_reset( _THIS );
272  }
273 
274  /*nV = QProblem_getNV( _THIS );
275  nC = QProblem_getNC( _THIS );
276 
277  DenseMatrix_print( _H );
278  DenseMatrix_print( _A );
279 
280  qpOASES_printV( _g,nV );
281  qpOASES_printV( _lb,nV );
282  qpOASES_printV( _ub,nV );
283  qpOASES_printV( _lbA,nC );
284  qpOASES_printV( _ubA,nC );*/
285 
286 
287  /* 2) Setup QP data. */
288  if ( QProblem_setupQPdataM( _THIS,_H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
289  return THROWERROR( RET_INVALID_ARGUMENTS );
290 
291  /* 3) Call to main initialisation routine (without any additional information). */
292  return QProblem_solveInitialQP( _THIS,0,0,0,0,0, nWSR,cputime );
293 }
294 
295 
296 /*
297  * i n i t
298  */
299 returnValue QProblem_init( QProblem* _THIS, real_t* const _H, const real_t* const _g, real_t* const _A,
300  const real_t* const _lb, const real_t* const _ub,
301  const real_t* const _lbA, const real_t* const _ubA,
302  int* nWSR, real_t* const cputime
303  )
304 {
305  if ( QProblem_getNV( _THIS ) == 0 )
306  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
307 
308  /* 1) Consistency check. */
309  if ( QProblem_isInitialised( _THIS ) == BT_TRUE )
310  {
311  THROWWARNING( RET_QP_ALREADY_INITIALISED );
312  QProblem_reset( _THIS );
313  }
314 
315  /* 2) Setup QP data. */
316  if ( QProblem_setupQPdata( _THIS,_H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
317  return THROWERROR( RET_INVALID_ARGUMENTS );
318 
319  /* 3) Call to main initialisation routine (without any additional information). */
320  return QProblem_solveInitialQP( _THIS,0,0,0,0,0, nWSR,cputime );
321 }
322 
323 
324 /*
325  * i n i t
326  */
327 returnValue QProblem_initF( QProblem* _THIS, const char* const H_file, const char* const g_file, const char* const A_file,
328  const char* const lb_file, const char* const ub_file,
329  const char* const lbA_file, const char* const ubA_file,
330  int* nWSR, real_t* const cputime
331  )
332 {
333  if ( QProblem_getNV( _THIS ) == 0 )
334  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
335 
336  /* 1) Consistency check. */
337  if ( QProblem_isInitialised( _THIS ) == BT_TRUE )
338  {
339  THROWWARNING( RET_QP_ALREADY_INITIALISED );
340  QProblem_reset( _THIS );
341  }
342 
343  /* 2) Setup QP data from files. */
344  if ( QProblem_setupQPdataFromFile( _THIS,H_file,g_file,A_file,lb_file,ub_file,lbA_file,ubA_file ) != SUCCESSFUL_RETURN )
345  return THROWERROR( RET_UNABLE_TO_READ_FILE );
346 
347  /* 3) Call to main initialisation routine (without any additional information). */
348  return QProblem_solveInitialQP( _THIS,0,0,0,0,0, nWSR,cputime );
349 }
350 
351 
352 /*
353  * i n i t
354  */
355 returnValue QProblem_initMW( QProblem* _THIS, DenseMatrix *_H, const real_t* const _g, DenseMatrix *_A,
356  const real_t* const _lb, const real_t* const _ub,
357  const real_t* const _lbA, const real_t* const _ubA,
358  int* nWSR, real_t* const cputime,
359  const real_t* const xOpt, const real_t* const yOpt,
360  Bounds* const guessedBounds, Constraints* const guessedConstraints,
361  const real_t* const _R
362  )
363 {
364  int i;
365  int nV = QProblem_getNV( _THIS );
366  int nC = QProblem_getNC( _THIS );
367 
368 
369  if ( nV == 0 )
370  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
371 
372  /* 1) Consistency checks. */
373  if ( QProblem_isInitialised( _THIS ) == BT_TRUE )
374  {
375  THROWWARNING( RET_QP_ALREADY_INITIALISED );
376  QProblem_reset( _THIS );
377  }
378 
379  if ( guessedBounds != 0 )
380  {
381  for( i=0; i<nV; ++i )
382  {
383  if ( Bounds_getStatus( guessedBounds,i ) == ST_UNDEFINED )
384  return THROWERROR( RET_INVALID_ARGUMENTS );
385  }
386  }
387 
388  if ( guessedConstraints != 0 )
389  {
390  for( i=0; i<nC; ++i )
391  if ( Constraints_getStatus( guessedConstraints,i ) == ST_UNDEFINED )
392  return THROWERROR( RET_INVALID_ARGUMENTS );
393  }
394 
395  /* exclude these possibilities in order to avoid inconsistencies */
396  if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
397  return THROWERROR( RET_INVALID_ARGUMENTS );
398 
399  if ( ( _R != 0 ) && ( ( xOpt != 0 ) || ( yOpt != 0 ) || ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
400  return THROWERROR( RET_NO_CHOLESKY_WITH_INITIAL_GUESS );
401 
402  /* 2) Setup QP data. */
403  if ( QProblem_setupQPdataM( _THIS,_H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
404  return THROWERROR( RET_INVALID_ARGUMENTS );
405 
406  /* 3) Call to main initialisation routine. */
407  return QProblem_solveInitialQP( _THIS,xOpt,yOpt,guessedBounds,guessedConstraints,_R, nWSR,cputime );
408 }
409 
410 
411 /*
412  * i n i t
413  */
414 returnValue QProblem_initW( QProblem* _THIS, real_t* const _H, const real_t* const _g, real_t* const _A,
415  const real_t* const _lb, const real_t* const _ub,
416  const real_t* const _lbA, const real_t* const _ubA,
417  int* nWSR, real_t* const cputime,
418  const real_t* const xOpt, const real_t* const yOpt,
419  Bounds* const guessedBounds, Constraints* const guessedConstraints,
420  const real_t* const _R
421  )
422 {
423  int i;
424  int nV = QProblem_getNV( _THIS );
425  int nC = QProblem_getNC( _THIS );
426 
427  if ( nV == 0 )
428  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
429 
430  /* 1) Consistency checks. */
431  if ( QProblem_isInitialised( _THIS ) == BT_TRUE )
432  {
433  THROWWARNING( RET_QP_ALREADY_INITIALISED );
434  QProblem_reset( _THIS );
435  }
436 
437  if ( guessedBounds != 0 )
438  {
439  for( i=0; i<nV; ++i )
440  {
441  if ( Bounds_getStatus( guessedBounds,i ) == ST_UNDEFINED )
442  return THROWERROR( RET_INVALID_ARGUMENTS );
443  }
444  }
445 
446  if ( guessedConstraints != 0 )
447  {
448  for( i=0; i<nC; ++i )
449  if ( Constraints_getStatus( guessedConstraints,i ) == ST_UNDEFINED )
450  return THROWERROR( RET_INVALID_ARGUMENTS );
451  }
452 
453  /* exclude these possibilities in order to avoid inconsistencies */
454  if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
455  return THROWERROR( RET_INVALID_ARGUMENTS );
456 
457  if ( ( _R != 0 ) && ( ( xOpt != 0 ) || ( yOpt != 0 ) || ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
458  return THROWERROR( RET_NO_CHOLESKY_WITH_INITIAL_GUESS );
459 
460  /* 2) Setup QP data. */
461  if ( QProblem_setupQPdata( _THIS,_H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
462  return THROWERROR( RET_INVALID_ARGUMENTS );
463 
464  /* 3) Call to main initialisation routine. */
465  return QProblem_solveInitialQP( _THIS,xOpt,yOpt,guessedBounds,guessedConstraints,_R, nWSR,cputime );
466 }
467 
468 
469 /*
470  * i n i t
471  */
472 returnValue QProblem_initFW( QProblem* _THIS, const char* const H_file, const char* const g_file, const char* const A_file,
473  const char* const lb_file, const char* const ub_file,
474  const char* const lbA_file, const char* const ubA_file,
475  int* nWSR, real_t* const cputime,
476  const real_t* const xOpt, const real_t* const yOpt,
477  Bounds* const guessedBounds, Constraints* const guessedConstraints,
478  const char* const R_file
479  )
480 {
481  int i;
482  int nV = QProblem_getNV( _THIS );
483  int nC = QProblem_getNC( _THIS );
484  returnValue returnvalue;
485 
486  if ( nV == 0 )
487  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
488 
489  /* 1) Consistency checks. */
490  if ( QProblem_isInitialised( _THIS ) == BT_TRUE )
491  {
492  THROWWARNING( RET_QP_ALREADY_INITIALISED );
493  QProblem_reset( _THIS );
494  }
495 
496  if ( guessedBounds != 0 )
497  {
498  for( i=0; i<nV; ++i )
499  {
500  if ( Bounds_getStatus( guessedBounds,i ) == ST_UNDEFINED )
501  return THROWERROR( RET_INVALID_ARGUMENTS );
502  }
503  }
504 
505  if ( guessedConstraints != 0 )
506  {
507  for( i=0; i<nC; ++i )
508  if ( Constraints_getStatus( guessedConstraints,i ) == ST_UNDEFINED )
509  return THROWERROR( RET_INVALID_ARGUMENTS );
510  }
511 
512  /* exclude these possibilities in order to avoid inconsistencies */
513  if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
514  return THROWERROR( RET_INVALID_ARGUMENTS );
515 
516  if ( ( R_file != 0 ) && ( ( xOpt != 0 ) || ( yOpt != 0 ) || ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
517  return THROWERROR( RET_NO_CHOLESKY_WITH_INITIAL_GUESS );
518 
519  /* 2) Setup QP data from files. */
520  if ( QProblem_setupQPdataFromFile( _THIS,H_file,g_file,A_file,lb_file,ub_file,lbA_file,ubA_file ) != SUCCESSFUL_RETURN )
521  return THROWERROR( RET_UNABLE_TO_READ_FILE );
522 
523  if ( R_file == 0 )
524  {
525  /* 3) Call to main initialisation routine. */
526  return QProblem_solveInitialQP( _THIS,xOpt,yOpt,guessedBounds,guessedConstraints,0, nWSR,cputime );
527  }
528  else
529  {
530  /* Also read Cholesky factor from file and store it directly into R [thus... */
531  returnvalue = qpOASES_readFromFileM( _THIS->R, nV,nV, R_file );
532  if ( returnvalue != SUCCESSFUL_RETURN )
533  return THROWWARNING( returnvalue );
534 
535  /* 3) Call to main initialisation routine. ...passing R here!] */
536  return QProblem_solveInitialQP( _THIS,xOpt,yOpt,guessedBounds,guessedConstraints,_THIS->R, nWSR,cputime );
537  }
538 }
539 
540 
541 /*
542  * s e t u p I n i t i a l C h o l e s k y
543  */
544 returnValue QProblem_setupInitialCholesky( QProblem* _THIS )
545 {
546  returnValue returnvalueCholesky;
547 
548  /* If regularisation shall be used, always regularise at beginning
549  * if initial working set is not empty. */
550  if ( ( QProblem_getNV(_THIS) != QProblem_getNFR(_THIS) - QProblem_getNFV(_THIS) ) && ( _THIS->options.enableRegularisation == BT_TRUE ) )
551  if ( QProblem_regulariseHessian( _THIS ) != SUCCESSFUL_RETURN )
552  return RET_INIT_FAILED_REGULARISATION;
553 
554  /* Factorise projected Hessian
555  * now handles all special cases (no active bounds/constraints, no nullspace) */
556  returnvalueCholesky = QProblem_computeProjectedCholesky( _THIS );
557 
558  /* If Hessian is not positive definite, regularise and try again. */
559  if ( returnvalueCholesky == RET_HESSIAN_NOT_SPD )
560  {
561  if ( QProblem_regulariseHessian( _THIS ) != SUCCESSFUL_RETURN )
562  return RET_INIT_FAILED_REGULARISATION;
563 
564  returnvalueCholesky = QProblem_computeProjectedCholesky( _THIS );
565  }
566 
567  if ( returnvalueCholesky != SUCCESSFUL_RETURN )
568  return RET_INIT_FAILED_CHOLESKY;
569 
570  _THIS->haveCholesky = BT_TRUE;
571  return SUCCESSFUL_RETURN;
572 }
573 
574 
575 /*
576  * h o t s t a r t
577  */
578 returnValue QProblem_hotstart( QProblem* _THIS,
579  const real_t* const g_new,
580  const real_t* const lb_new, const real_t* const ub_new,
581  const real_t* const lbA_new, const real_t* const ubA_new,
582  int* nWSR, real_t* const cputime
583  )
584 {
585  returnValue returnvalue = SUCCESSFUL_RETURN;
586  int i, nActiveFar;
587  int nV = QProblem_getNV( _THIS );
588  int nC = QProblem_getNC( _THIS );
589 
590  int nWSR_max = *nWSR;
591  int nWSR_performed = 0;
592 
593  real_t cputime_remaining = QPOASES_INFTY, *pcputime_rem;
594  real_t cputime_needed = 0.0;
595 
596  real_t farbound = _THIS->options.initialFarBounds;
597 
598  BooleanType isFirstCall = BT_TRUE;
599 
600  myStatic real_t ub_new_far[NVMAX];
601  myStatic real_t lb_new_far[NVMAX];
602  myStatic real_t ubA_new_far[NCMAX];
603  myStatic real_t lbA_new_far[NCMAX];
604 
605  real_t tol;
606 
607  if ( QProblem_getNV( _THIS ) == 0 )
608  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
609 
610  /* Simple check for consistency of bounds and constraints. */
611  if ( QProblem_areBoundsConsistent(_THIS,lb_new, ub_new, lbA_new, ubA_new) != SUCCESSFUL_RETURN )
612  return QProblem_setInfeasibilityFlag(_THIS,returnvalue,BT_TRUE);
613 
614  ++(_THIS->count);
615 
616  /*qpOASES_printNV( g_new,nV,"g" );
617  qpOASES_printNV( lb_new,nV,"lb" );
618  qpOASES_printNV( ub_new,nV,"ub" );
619  qpOASES_printNV( lbA_new,nC,"lbA" );
620  qpOASES_printNV( ubA_new,nC,"ubA" );*/
621 
622  /*QProblem_writeQpDataIntoMatFile( _THIS,"qpData.mat" );*/
623  /*QProblem_writeQpWorkspaceIntoMatFile( _THIS,"qpWorkspace.mat" );*/
624 
625 
626  if ( _THIS->haveCholesky == BT_FALSE )
627  {
628  returnvalue = QProblem_setupInitialCholesky( _THIS );
629  if (returnvalue != SUCCESSFUL_RETURN)
630  return THROWERROR(returnvalue);
631  }
632 
633 
634  if ( _THIS->options.enableFarBounds == BT_FALSE )
635  {
636  /* Automatically call standard solveQP if regularisation is not active. */
637  returnvalue = QProblem_solveRegularisedQP( _THIS,g_new,lb_new,ub_new,lbA_new,ubA_new,
638  nWSR,cputime,0,
639  isFirstCall
640  );
641  }
642  else
643  {
644  /* possibly extend initial far bounds to largest bound/constraint data */
645  if (ub_new)
646  for (i = 0; i < nV; i++)
647  if ((ub_new[i] < QPOASES_INFTY) && (ub_new[i] > farbound)) farbound = ub_new[i];
648  if (lb_new)
649  for (i = 0; i < nV; i++)
650  if ((lb_new[i] > -QPOASES_INFTY) && (lb_new[i] < -farbound)) farbound = -lb_new[i];
651  if (ubA_new)
652  for (i = 0; i < nC; i++)
653  if ((ubA_new[i] < QPOASES_INFTY) && (ubA_new[i] > farbound)) farbound = ubA_new[i];
654  if (lbA_new)
655  for (i = 0; i < nC; i++)
656  if ((lbA_new[i] > -QPOASES_INFTY) && (lbA_new[i] < -farbound)) farbound = -lbA_new[i];
657 
658  QProblem_updateFarBounds( _THIS,farbound,nV+nC,
659  lb_new,lb_new_far, ub_new,ub_new_far,
660  lbA_new,lbA_new_far, ubA_new,ubA_new_far
661  );
662 
663  for ( ;; )
664  {
665  *nWSR = nWSR_max;
666  if ( cputime != 0 )
667  {
668  cputime_remaining = *cputime - cputime_needed;
669  pcputime_rem = &cputime_remaining;
670  }
671  else
672  pcputime_rem = 0;
673 
674  /* Automatically call standard solveQP if regularisation is not active. */
675  returnvalue = QProblem_solveRegularisedQP( _THIS,g_new,lb_new_far,ub_new_far,lbA_new_far,ubA_new_far,
676  nWSR,pcputime_rem,nWSR_performed,
677  isFirstCall
678  );
679 
680  nWSR_performed = *nWSR;
681  cputime_needed += cputime_remaining;
682  isFirstCall = BT_FALSE;
683 
684  /* Check for active far-bounds and move them away */
685  nActiveFar = 0;
686  farbound *= _THIS->options.growFarBounds;
687 
688  if ( _THIS->infeasible == BT_TRUE )
689  {
690  if ( farbound >= QPOASES_INFTY )
691  {
692  returnvalue = RET_HOTSTART_STOPPED_INFEASIBILITY;
693  break; /* goto farewell; */
694  }
695 
696  QProblem_updateFarBounds( _THIS,farbound,nV+nC,
697  lb_new,lb_new_far, ub_new,ub_new_far,
698  lbA_new,lbA_new_far, ubA_new,ubA_new_far
699  );
700  }
701  else if ( _THIS->status == QPS_SOLVED )
702  {
703  tol = farbound/_THIS->options.growFarBounds * _THIS->options.boundTolerance;
704 
705  for ( i=0; i<nV; ++i )
706  {
707  if ( ( ( lb_new == 0 ) || ( lb_new_far[i] > lb_new[i] ) ) && ( qpOASES_getAbs ( lb_new_far[i] - _THIS->x[i] ) < tol ) )
708  ++nActiveFar;
709  if ( ( ( ub_new == 0 ) || ( ub_new_far[i] < ub_new[i] ) ) && ( qpOASES_getAbs ( ub_new_far[i] - _THIS->x[i] ) < tol ) )
710  ++nActiveFar;
711  }
712  for ( i=0; i<nC; ++i )
713  {
714  if ( ( ( lbA_new == 0 ) || ( lbA_new_far[i] > lbA_new[i] ) ) && ( qpOASES_getAbs ( lbA_new_far[i] - _THIS->Ax[i] ) < tol ) )
715  ++nActiveFar;
716  if ( ( ( ubA_new == 0 ) || ( ubA_new_far[i] < ubA_new[i] ) ) && ( qpOASES_getAbs ( ubA_new_far[i] - _THIS->Ax[i] ) < tol ) )
717  ++nActiveFar;
718  }
719 
720  if ( nActiveFar == 0 )
721  break;
722 
723  _THIS->status = QPS_HOMOTOPYQPSOLVED;
724 
725  if ( farbound >= QPOASES_INFTY )
726  {
727  _THIS->unbounded = BT_TRUE;
728  returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
729  goto farewell;
730  }
731 
732  QProblem_updateFarBounds( _THIS,farbound,nV+nC,
733  lb_new,lb_new_far, ub_new,ub_new_far,
734  lbA_new,lbA_new_far, ubA_new,ubA_new_far
735  );
736  }
737  else
738  {
739  /* some other error */
740  break;
741  }
742 
743  /* advance ramp offset to avoid Ramping cycles */
744  (_THIS->rampOffset)++;
745  }
746 
747  farewell:
748  if ( cputime != 0 )
749  *cputime = cputime_needed;
750  }
751 
752  return ( returnvalue != SUCCESSFUL_RETURN ) ? THROWERROR( returnvalue ) : returnvalue;
753 }
754 
755 
756 /*
757  * h o t s t a r t
758  */
759 returnValue QProblem_hotstartF( QProblem* _THIS, const char* const g_file,
760  const char* const lb_file, const char* const ub_file,
761  const char* const lbA_file, const char* const ubA_file,
762  int* nWSR, real_t* const cputime
763  )
764 {
765  int nV = QProblem_getNV( _THIS );
766  returnValue returnvalue;
767 
768  /* 1) Allocate memory (if bounds exist). */
769  myStatic real_t g_new[NVMAX];
770  myStatic real_t lb_new[NVMAX];
771  myStatic real_t ub_new[NVMAX];
772  myStatic real_t lbA_new[NCMAX];
773  myStatic real_t ubA_new[NCMAX];
774 
775 
776  if ( nV == 0 )
777  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
778 
779  /* consistency check */
780  if ( g_file == 0 )
781  return THROWERROR( RET_INVALID_ARGUMENTS );
782 
783 
784  /* 2) Load new QP vectors from file. */
785  returnvalue = QProblem_loadQPvectorsFromFile( _THIS,g_file,lb_file,ub_file,lbA_file,ubA_file,
786  g_new,lb_new,ub_new,lbA_new,ubA_new
787  );
788  if ( returnvalue != SUCCESSFUL_RETURN )
789  return THROWERROR( RET_UNABLE_TO_READ_FILE );
790 
791  /* 3) Actually perform hotstart. */
792  returnvalue = QProblem_hotstart( _THIS,g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
793 
794  return returnvalue;
795 }
796 
797 
798 /*
799  * h o t s t a r t
800  */
801 returnValue QProblem_hotstartW( QProblem* _THIS, const real_t* const g_new,
802  const real_t* const lb_new, const real_t* const ub_new,
803  const real_t* const lbA_new, const real_t* const ubA_new,
804  int* nWSR, real_t* const cputime,
805  Bounds* const guessedBounds, Constraints* const guessedConstraints
806  )
807 {
808  int nV = QProblem_getNV( _THIS );
809 
810  real_t starttime = 0.0;
811  real_t auxTime = 0.0;
812 
813  Bounds* actualGuessedBounds;
814  Constraints* actualGuessedConstraints;
815 
816  returnValue returnvalue;
817 
818  if ( nV == 0 )
819  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
820 
821 
822  /* start runtime measurement */
823  if ( cputime != 0 )
824  starttime = qpOASES_getCPUtime( );
825 
826 
827  /* 1) Update working sets according to guesses for working sets of bounds and constraints. */
828  if ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) )
829  {
830  if ( cputime != 0 )
831  starttime = qpOASES_getCPUtime( );
832 
833  actualGuessedBounds = ( guessedBounds != 0 ) ? guessedBounds : &(_THIS->bounds);
834  actualGuessedConstraints = ( guessedConstraints != 0 ) ? guessedConstraints : &(_THIS->constraints);
835 
836  if ( QProblem_setupAuxiliaryQP( _THIS,actualGuessedBounds,actualGuessedConstraints ) != SUCCESSFUL_RETURN )
837  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
838 
839  /* Allow only remaining CPU time for usual hotstart. */
840  if ( cputime != 0 )
841  {
842  auxTime = qpOASES_getCPUtime( ) - starttime;
843  *cputime -= auxTime;
844  }
845  }
846 
847  _THIS->status = QPS_AUXILIARYQPSOLVED;
848 
849 
850  /* 2) Perform usual homotopy. */
851  returnvalue = QProblem_hotstart( _THIS,g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
852 
853  /* stop runtime measurement */
854  if ( cputime != 0 )
855  *cputime += auxTime;
856 
857  return returnvalue;
858 }
859 
860 
861 /*
862  * h o t s t a r t
863  */
864 returnValue QProblem_hotstartFW( QProblem* _THIS, const char* const g_file,
865  const char* const lb_file, const char* const ub_file,
866  const char* const lbA_file, const char* const ubA_file,
867  int* nWSR, real_t* const cputime,
868  Bounds* const guessedBounds, Constraints* const guessedConstraints
869  )
870 {
871  int nV = QProblem_getNV( _THIS );
872  returnValue returnvalue;
873 
874  /* 1) Allocate memory (if bounds exist). */
875  myStatic real_t g_new[NVMAX];
876  myStatic real_t lb_new[NVMAX];
877  myStatic real_t ub_new[NVMAX];
878  myStatic real_t lbA_new[NCMAX];
879  myStatic real_t ubA_new[NCMAX];
880 
881 
882  if ( nV == 0 )
883  return THROWERROR( RET_QPOBJECT_NOT_SETUP );
884 
885  /* consistency check */
886  if ( g_file == 0 )
887  return THROWERROR( RET_INVALID_ARGUMENTS );
888 
889  /* 2) Load new QP vectors from file. */
890  returnvalue = QProblem_loadQPvectorsFromFile( _THIS,g_file,lb_file,ub_file,lbA_file,ubA_file,
891  g_new,lb_new,ub_new,lbA_new,ubA_new
892  );
893  if (returnvalue != SUCCESSFUL_RETURN) {
894  returnvalue = RET_UNABLE_TO_READ_FILE;
895  goto farewell;
896  }
897 
898  /* 3) Actually perform hotstart using initialised homotopy. */
899  returnvalue = QProblem_hotstartW( _THIS,g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,
900  guessedBounds,guessedConstraints
901  );
902 farewell:
903  return ( returnvalue != SUCCESSFUL_RETURN ) ? THROWERROR ( returnvalue ) : returnvalue;
904 }
905 
906 
907 /*
908  *
909  */
910 returnValue QProblem_solveCurrentEQP( QProblem* _THIS,
911  const int n_rhs,
912  const real_t* g_in,
913  const real_t* lb_in,
914  const real_t* ub_in,
915  const real_t* lbA_in,
916  const real_t* ubA_in,
917  real_t* x_out,
918  real_t* y_out
919  )
920 {
921  returnValue returnvalue = SUCCESSFUL_RETURN;
922  int ii, jj;
923  int nV = QProblem_getNV( _THIS );
924  int nC = QProblem_getNC( _THIS );
925  int nFR = QProblem_getNFR( _THIS );
926  int nFX = QProblem_getNFX( _THIS );
927  int nAC = QProblem_getNAC( _THIS );
928 
929  myStatic real_t delta_xFX[NVMAX];
930  myStatic real_t delta_xFR[NVMAX];
931  myStatic real_t delta_yAC[NCMAX];
932  myStatic real_t delta_yFX[NVMAX];
933 
934  /* 1) Determine index arrays. */
935  int* FR_idx;
936  int* FX_idx;
937  int* AC_idx;
938 
939  if ( ( x_out == 0 ) || ( y_out == 0 ) )
940  return THROWERROR( RET_INVALID_ARGUMENTS );
941 
942  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
943  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
944  Indexlist_getNumberArray( Constraints_getActive( &(_THIS->constraints) ),&AC_idx );
945 
946  for ( ii = 0 ; ii < (nV+nC)*n_rhs; ++ii )
947  y_out[ii] = 0.0;
948 
949  for ( ii = 0 ; ii < n_rhs; ++ii )
950  {
951  returnvalue = QProblem_determineStepDirection( _THIS,
952  g_in, lbA_in, ubA_in, lb_in, ub_in, BT_FALSE, BT_FALSE,
953  delta_xFX, delta_xFR, delta_yAC, delta_yFX );
954 
955  for ( jj = 0; jj < nFX; ++jj )
956  x_out[FX_idx[jj]] = delta_xFX[jj];
957  for ( jj = 0; jj < nFR; ++jj )
958  x_out[FR_idx[jj]] = delta_xFR[jj];
959  for ( jj = 0; jj < nFX; ++jj )
960  y_out[FX_idx[jj]] = delta_yFX[jj];
961  for ( jj = 0; jj < nAC; ++jj )
962  y_out[nV+AC_idx[jj]] = delta_yAC[jj];
963 
964  g_in += nV;
965  lb_in += nV;
966  ub_in += nV;
967  lbA_in += nC;
968  ubA_in += nC;
969  x_out += nV;
970  y_out += nV+nC;
971  }
972 
973  return returnvalue;
974 }
975 
976 
977 
978 /*
979  * g e t W o r k i n g S e t
980  */
981 returnValue QProblem_getWorkingSet( QProblem* _THIS, real_t* workingSet )
982 {
983  int nV = QProblem_getNV( _THIS );
984 
985  if ( workingSet == 0 )
986  return THROWERROR( RET_INVALID_ARGUMENTS );
987 
988  /* At which limit are the bounds active? */
989  QProblem_getWorkingSetBounds( _THIS,workingSet );
990 
991  /* At which limit are the contraints active? */
992  QProblem_getWorkingSetConstraints( _THIS,&(workingSet[nV]) );
993 
994  return SUCCESSFUL_RETURN;
995 }
996 
997 
998 /*
999  * g e t W o r k i n g S e t B o u n d s
1000  */
1001 returnValue QProblem_getWorkingSetBounds( QProblem* _THIS, real_t* workingSetB )
1002 {
1003  int i;
1004  int nV = QProblem_getNV( _THIS );
1005 
1006  /* At which limit is the bound active? */
1007  for (i = 0; i < nV; i++) {
1008  switch ( Bounds_getStatus( &(_THIS->bounds),i ) ) {
1009  case ST_LOWER: workingSetB[i] = -1.0; break;
1010  case ST_UPPER: workingSetB[i] = +1.0; break;
1011  default: workingSetB[i] = 0.0; break;
1012  }
1013  }
1014 
1015  return SUCCESSFUL_RETURN;
1016 }
1017 
1018 
1019 /*
1020  * g e t W o r k i n g S e t C o n s t r a i n t s
1021  */
1022 returnValue QProblem_getWorkingSetConstraints( QProblem* _THIS, real_t* workingSetC )
1023 {
1024  int i;
1025  int nC = QProblem_getNC( _THIS );
1026 
1027  if ( workingSetC == 0 )
1028  return THROWERROR( RET_INVALID_ARGUMENTS );
1029 
1030  for ( i=0; i<nC; ++i )
1031  {
1032  switch ( Constraints_getStatus( &(_THIS->constraints),i ) )
1033  {
1034  case ST_LOWER: workingSetC[i] = -1.0; break;
1035  case ST_UPPER: workingSetC[i] = +1.0; break;
1036  default: workingSetC[i] = 0.0; break;
1037  }
1038  }
1039 
1040  return SUCCESSFUL_RETURN;
1041 }
1042 
1043 
1044 
1045 /*
1046  * g e t N Z
1047  */
1048 int QProblem_getNZ( QProblem* _THIS )
1049 {
1050  /* nZ = nFR - nAC */
1051  return QProblem_getNFR( _THIS ) - QProblem_getNAC( _THIS );
1052 }
1053 
1054 
1055 /*
1056  * g e t D u a l S o l u t i o n
1057  */
1058 returnValue QProblem_getDualSolution( QProblem* _THIS, real_t* const yOpt )
1059 {
1060  int i;
1061 
1062  for( i=0; i<QProblem_getNV( _THIS )+QProblem_getNC( _THIS ); ++i )
1063  yOpt[i] = _THIS->y[i];
1064 
1065  /* return optimal dual solution vector
1066  * only if current QP has been solved */
1067  if ( ( QProblem_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
1068  ( QProblem_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
1069  ( QProblem_getStatus( _THIS ) == QPS_SOLVED ) )
1070  {
1071  return SUCCESSFUL_RETURN;
1072  }
1073  else
1074  {
1075  return RET_QP_NOT_SOLVED;
1076  }
1077 }
1078 
1079 
1080 
1081 /*
1082  * s e t C o n s t r a i n t P r o d u c t
1083  */
1084 returnValue QProblem_setConstraintProduct( QProblem* _THIS, ConstraintProduct _constraintProduct )
1085 {
1086  _THIS->constraintProduct = _constraintProduct;
1087 
1088  return SUCCESSFUL_RETURN;
1089 }
1090 
1091 
1092 /*
1093  * g e t O b j V a l
1094  */
1095 real_t QProblem_getObjVal( QProblem* _THIS )
1096 {
1097  real_t objVal;
1098 
1099  /* calculated optimal objective function value
1100  * only if current QP has been solved */
1101  if ( ( QProblem_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
1102  ( QProblem_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
1103  ( QProblem_getStatus( _THIS ) == QPS_SOLVED ) )
1104  {
1105  objVal = QProblem_getObjValX( _THIS,_THIS->x );
1106  }
1107  else
1108  {
1109  objVal = QPOASES_INFTY;
1110  }
1111 
1112  return objVal;
1113 }
1114 
1115 
1116 /*
1117  * g e t O b j V a l
1118  */
1119 real_t QProblem_getObjValX( QProblem* _THIS, const real_t* const _x )
1120 {
1121  int i;
1122  int nV = QProblem_getNV( _THIS );
1123 
1124  real_t objVal = 0.0;
1125  myStatic real_t Hx[NVMAX];
1126 
1127  if ( nV == 0 )
1128  return 0.0;
1129 
1130  for( i=0; i<nV; ++i )
1131  objVal += _x[i]*_THIS->g[i];
1132 
1133  switch ( _THIS->hessianType )
1134  {
1135  case HST_ZERO:
1136  break;
1137 
1138  case HST_IDENTITY:
1139  for( i=0; i<nV; ++i )
1140  objVal += 0.5*_x[i]*_x[i];
1141  break;
1142 
1143  default:
1144  DenseMatrix_times(_THIS->H,1, 1.0, _x, nV, 0.0, Hx, nV);
1145  for( i=0; i<nV; ++i )
1146  objVal += 0.5*_x[i]*Hx[i];
1147  break;
1148  }
1149 
1150  /* When using regularisation, the objective function value
1151  * needs to be modified as follows:
1152  * objVal = objVal - 0.5*_x*(Hmod-H)*_x - _x'*(gMod-g)
1153  * = objVal - 0.5*_x*eps*_x * - _x'*(-eps*_x)
1154  * = objVal + 0.5*_x*eps*_x */
1155  if ( QProblem_usingRegularisation( _THIS ) == BT_TRUE )
1156  {
1157  for( i=0; i<nV; ++i )
1158  objVal += 0.5*_x[i]*_THIS->regVal*_x[i];
1159  }
1160 
1161  return objVal;
1162 }
1163 
1164 
1165 /*
1166  * g e t P r i m a l S o l u t i o n
1167  */
1168 returnValue QProblem_getPrimalSolution( QProblem* _THIS, real_t* const xOpt )
1169 {
1170  int i;
1171 
1172  /* return optimal primal solution vector
1173  * only if current QP has been solved */
1174  if ( ( QProblem_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
1175  ( QProblem_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
1176  ( QProblem_getStatus( _THIS ) == QPS_SOLVED ) )
1177  {
1178  for( i=0; i<QProblem_getNV( _THIS ); ++i )
1179  xOpt[i] = _THIS->x[i];
1180 
1181  return SUCCESSFUL_RETURN;
1182  }
1183  else
1184  {
1185  return RET_QP_NOT_SOLVED;
1186  }
1187 }
1188 
1189 
1190 /*
1191  * s e t P r i n t L e v e l
1192  */
1193 returnValue QProblem_setPrintLevel( QProblem* _THIS, PrintLevel _printLevel )
1194 {
1195  #ifndef __SUPPRESSANYOUTPUT__
1196  #ifndef __MATLAB__
1197  if ( ( _THIS->options.printLevel == PL_HIGH ) && ( _THIS->options.printLevel != _printLevel ) )
1198  THROWINFO( RET_PRINTLEVEL_CHANGED );
1199  #endif /* __MATLAB__ */
1200  _THIS->options.printLevel = _printLevel;
1201  #else
1202  _THIS->options.printLevel = PL_NONE;
1203  #endif /* __SUPPRESSANYOUTPUT__ */
1204 
1205  /* update message handler preferences */
1206  switch ( _THIS->options.printLevel )
1207  {
1208  case PL_NONE:
1209  MessageHandling_setErrorVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
1210  MessageHandling_setWarningVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
1211  MessageHandling_setInfoVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
1212  break;
1213 
1214  case PL_DEBUG_ITER:
1215  case PL_TABULAR:
1216  case PL_LOW:
1217  MessageHandling_setErrorVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
1218  MessageHandling_setWarningVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
1219  MessageHandling_setInfoVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
1220  break;
1221 
1222  case PL_MEDIUM:
1223  MessageHandling_setErrorVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
1224  MessageHandling_setWarningVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
1225  MessageHandling_setInfoVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_HIDDEN );
1226  break;
1227 
1228  default: /* PL_HIGH */
1229  MessageHandling_setErrorVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
1230  MessageHandling_setWarningVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
1231  MessageHandling_setInfoVisibilityStatus( qpOASES_getGlobalMessageHandler(),VS_VISIBLE );
1232  break;
1233  }
1234 
1235  return SUCCESSFUL_RETURN;
1236 }
1237 
1238 
1239 
1240 returnValue QProblem_printOptions( QProblem* _THIS )
1241 {
1242  return Options_print( &(_THIS->options) );
1243 }
1244 
1245 
1246 /*
1247  * p r i n t P r o p e r t i e s
1248  */
1249 returnValue QProblem_printProperties( QProblem* _THIS )
1250 {
1251  #ifndef __SUPPRESSANYOUTPUT__
1252 
1253  myStatic char myPrintfString[QPOASES_MAX_STRING_LENGTH];
1254 
1255  /* Do not print properties if print level is set to none! */
1256  if ( _THIS->options.printLevel == PL_NONE )
1257  return SUCCESSFUL_RETURN;
1258 
1259  qpOASES_myPrintf( "\n################# qpOASES -- QP PROPERTIES #################\n" );
1260  qpOASES_myPrintf( "\n" );
1261 
1262  /* 1) Variables properties. */
1263  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "Number of Variables: %4.1d\n",QProblem_getNV( _THIS ) );
1264  qpOASES_myPrintf( myPrintfString );
1265 
1266  if ( Bounds_hasNoLower( &(_THIS->bounds) ) == BT_TRUE )
1267  qpOASES_myPrintf( "Variables are not bounded from below.\n" );
1268  else
1269  qpOASES_myPrintf( "Variables are bounded from below.\n" );
1270 
1271  if ( Bounds_hasNoUpper( &(_THIS->bounds) ) == BT_TRUE )
1272  qpOASES_myPrintf( "Variables are not bounded from above.\n" );
1273  else
1274  qpOASES_myPrintf( "Variables are bounded from above.\n" );
1275 
1276  qpOASES_myPrintf( "\n" );
1277 
1278 
1279  /* 2) Constraints properties. */
1280  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "Total number of Constraints: %4.1d\n",QProblem_getNC( _THIS ) );
1281  qpOASES_myPrintf( myPrintfString );
1282 
1283  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "Number of Equality Constraints: %4.1d\n",QProblem_getNEC( _THIS ) );
1284  qpOASES_myPrintf( myPrintfString );
1285 
1286  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "Number of Inequality Constraints: %4.1d\n",QProblem_getNC( _THIS )-QProblem_getNEC( _THIS ) );
1287  qpOASES_myPrintf( myPrintfString );
1288 
1289  if ( QProblem_getNC( _THIS ) > 0 )
1290  {
1291  if ( Constraints_hasNoLower( &(_THIS->constraints) ) == BT_TRUE )
1292  qpOASES_myPrintf( "Constraints are not bounded from below.\n" );
1293  else
1294  qpOASES_myPrintf( "Constraints are bounded from below.\n" );
1295 
1296  if ( Constraints_hasNoUpper( &(_THIS->constraints) ) == BT_TRUE )
1297  qpOASES_myPrintf( "Constraints are not bounded from above.\n" );
1298  else
1299  qpOASES_myPrintf( "Constraints are bounded from above.\n" );
1300  }
1301 
1302  qpOASES_myPrintf( "\n" );
1303 
1304 
1305  /* 3) Further properties. */
1306  switch ( _THIS->hessianType )
1307  {
1308  case HST_ZERO:
1309  qpOASES_myPrintf( "Hessian is zero matrix (i.e. actually an LP is solved).\n" );
1310  break;
1311 
1312  case HST_IDENTITY:
1313  qpOASES_myPrintf( "Hessian is identity matrix.\n" );
1314  break;
1315 
1316  case HST_POSDEF:
1317  qpOASES_myPrintf( "Hessian matrix is (strictly) positive definite.\n" );
1318  break;
1319 
1320  case HST_POSDEF_NULLSPACE:
1321  qpOASES_myPrintf( "Hessian matrix is positive definite on null space of active constraints.\n" );
1322  break;
1323 
1324  case HST_SEMIDEF:
1325  qpOASES_myPrintf( "Hessian matrix is positive semi-definite.\n" );
1326  break;
1327 
1328  case HST_INDEF:
1329  qpOASES_myPrintf( "Hessian matrix is indefinite.\n" );
1330  break;
1331 
1332  default:
1333  qpOASES_myPrintf( "Hessian matrix has unknown type.\n" );
1334  break;
1335  }
1336 
1337  if ( _THIS->infeasible == BT_TRUE )
1338  qpOASES_myPrintf( "QP was found to be infeasible.\n" );
1339  else
1340  qpOASES_myPrintf( "QP seems to be feasible.\n" );
1341 
1342  if ( _THIS->unbounded == BT_TRUE )
1343  qpOASES_myPrintf( "QP was found to be unbounded from below.\n" );
1344  else
1345  qpOASES_myPrintf( "QP seems to be bounded from below.\n" );
1346 
1347  qpOASES_myPrintf( "\n" );
1348 
1349 
1350  /* 4) QP object properties. */
1351  switch ( _THIS->status )
1352  {
1353  case QPS_NOTINITIALISED:
1354  qpOASES_myPrintf( "Status of QP object: freshly instantiated or reset.\n" );
1355  break;
1356 
1357  case QPS_PREPARINGAUXILIARYQP:
1358  qpOASES_myPrintf( "Status of QP object: an auxiliary QP is currently setup.\n" );
1359  break;
1360 
1361  case QPS_AUXILIARYQPSOLVED:
1362  qpOASES_myPrintf( "Status of QP object: an auxilary QP was solved.\n" );
1363  break;
1364 
1365  case QPS_PERFORMINGHOMOTOPY:
1366  qpOASES_myPrintf( "Status of QP object: a homotopy step is performed.\n" );
1367  break;
1368 
1369  case QPS_HOMOTOPYQPSOLVED:
1370  qpOASES_myPrintf( "Status of QP object: an intermediate QP along the homotopy path was solved.\n" );
1371  break;
1372 
1373  case QPS_SOLVED:
1374  qpOASES_myPrintf( "Status of QP object: solution of the actual QP was found.\n" );
1375  break;
1376  }
1377 
1378  switch ( _THIS->options.printLevel )
1379  {
1380  case PL_DEBUG_ITER:
1381  qpOASES_myPrintf( "Print level of QP object is set to display a tabular output for debugging.\n" );
1382  break;
1383 
1384  case PL_TABULAR:
1385  qpOASES_myPrintf( "Print level of QP object is set to display a tabular output.\n" );
1386  break;
1387 
1388  case PL_LOW:
1389  qpOASES_myPrintf( "Print level of QP object is low, i.e. only error are printed.\n" );
1390  break;
1391 
1392  case PL_MEDIUM:
1393  qpOASES_myPrintf( "Print level of QP object is medium, i.e. error and warnings are printed.\n" );
1394  break;
1395 
1396  case PL_HIGH:
1397  qpOASES_myPrintf( "Print level of QP object is high, i.e. all available output is printed.\n" );
1398  break;
1399 
1400  default:
1401  break;
1402  }
1403 
1404  qpOASES_myPrintf( "\n" );
1405 
1406  #endif /* __SUPPRESSANYOUTPUT__ */
1407 
1408  return SUCCESSFUL_RETURN;
1409 }
1410 
1411 
1412 
1413 /*****************************************************************************
1414  * P R O T E C T E D *
1415  *****************************************************************************/
1416 
1417 
1418 /*
1419  * d e t e r m i n e H e s s i a n T y p e
1420  */
1421 returnValue QProblem_determineHessianType( QProblem* _THIS )
1422 {
1423  int i;
1424  real_t curDiag;
1425  BooleanType isIdentity, isZero;
1426 
1427  int nV = QProblem_getNV( _THIS );
1428 
1429  /* if Hessian type has been set by user, do NOT change it! */
1430  switch ( _THIS->hessianType )
1431  {
1432  case HST_ZERO:
1433  /* ensure regularisation as default options do not always solve LPs */
1434  if ( _THIS->options.enableRegularisation == BT_FALSE )
1435  {
1436  _THIS->options.enableRegularisation = BT_TRUE;
1437  _THIS->options.numRegularisationSteps = 1;
1438  }
1439  return SUCCESSFUL_RETURN;
1440 
1441  case HST_IDENTITY:
1442  return SUCCESSFUL_RETURN;
1443 
1444  case HST_POSDEF:
1445  case HST_POSDEF_NULLSPACE:
1446  case HST_SEMIDEF:
1447  case HST_INDEF:
1448  /* if H == 0, continue to reset hessianType to HST_ZERO
1449  * to avoid segmentation faults! */
1450  if ( _THIS->H != 0 )
1451  return SUCCESSFUL_RETURN;
1452 
1453  default:
1454  /* HST_UNKNOWN, continue */
1455  break;
1456  }
1457 
1458  /* if Hessian has not been allocated, assume it to be all zeros! */
1459  if ( _THIS->H == 0 )
1460  {
1461  _THIS->hessianType = HST_ZERO;
1462  THROWINFO( RET_ZERO_HESSIAN_ASSUMED );
1463 
1464  /* ensure regularisation as default options do not always solve LPs */
1465  if ( _THIS->options.enableRegularisation == BT_FALSE )
1466  _THIS->options.enableRegularisation = BT_TRUE;
1467 
1468  return SUCCESSFUL_RETURN;
1469  }
1470 
1471  /* 1) If Hessian has outer-diagonal elements,
1472  * Hessian is assumed to be positive definite. */
1473  _THIS->hessianType = HST_POSDEF;
1474  if (DenseMatrix_isDiag(_THIS->H) == BT_FALSE)
1475  return SUCCESSFUL_RETURN;
1476 
1477  /* 2) Otherwise it is diagonal and test for identity or zero matrix is performed. */
1478  isIdentity = BT_TRUE;
1479  isZero = BT_TRUE;
1480 
1481  for ( i=0; i<nV; ++i )
1482  {
1483  curDiag = DenseMatrix_diag( _THIS->H,i );
1484  if ( curDiag >= QPOASES_INFTY )
1485  return RET_DIAGONAL_NOT_INITIALISED;
1486 
1487  if ( curDiag < -QPOASES_ZERO )
1488  {
1489  _THIS->hessianType = HST_INDEF;
1490  if ( _THIS->options.enableFlippingBounds == BT_FALSE )
1491  return THROWERROR( RET_HESSIAN_INDEFINITE );
1492  else
1493  return SUCCESSFUL_RETURN;
1494  }
1495 
1496  if ( qpOASES_getAbs( curDiag - 1.0 ) > QPOASES_EPS )
1497  isIdentity = BT_FALSE;
1498 
1499  if ( qpOASES_getAbs( curDiag ) > QPOASES_EPS )
1500  isZero = BT_FALSE;
1501  }
1502 
1503  if ( isIdentity == BT_TRUE )
1504  _THIS->hessianType = HST_IDENTITY;
1505 
1506  if ( isZero == BT_TRUE )
1507  {
1508  _THIS->hessianType = HST_ZERO;
1509 
1510  /* ensure regularisation as default options do not always solve LPs */
1511  if ( _THIS->options.enableRegularisation == BT_FALSE )
1512  {
1513  _THIS->options.enableRegularisation = BT_TRUE;
1514  _THIS->options.numRegularisationSteps = 1;
1515  }
1516  }
1517 
1518  return SUCCESSFUL_RETURN;
1519 }
1520 
1521 
1522 /*
1523  * c o m p u t e C h o l e s k y
1524  */
1525 returnValue QProblemBCPY_computeCholesky( QProblem* _THIS )
1526 {
1527  int i, j;
1528  int nV = QProblem_getNV( _THIS );
1529  int nFR = QProblem_getNFR( _THIS );
1530 
1531  int* FR_idx;
1532 
1533  long info = 0;
1534  unsigned long _nFR = (unsigned long)nFR, _nV = NVMAX;
1535 
1536  /* 1) Initialises R with all zeros. */
1537  for( i=0; i<NVMAX*NVMAX; ++i )
1538  _THIS->R[i] = 0.0;
1539 
1540  /* 2) Calculate Cholesky decomposition of H (projected to free variables). */
1541  switch ( _THIS->hessianType )
1542  {
1543  case HST_ZERO:
1544  /* if Hessian is zero matrix and it has been regularised,
1545  * its Cholesky factor is the identity matrix scaled by sqrt(eps). */
1546  if ( QProblem_usingRegularisation( _THIS ) == BT_TRUE )
1547  {
1548  for( i=0; i<nV; ++i )
1549  RR(i,i) = qpOASES_getSqrt( _THIS->regVal );
1550  }
1551  else
1552  {
1553  return THROWERROR( RET_CHOLESKY_OF_ZERO_HESSIAN );
1554  }
1555  break;
1556 
1557  case HST_IDENTITY:
1558  /* if Hessian is identity, so is its Cholesky factor. */
1559  for( i=0; i<nV; ++i )
1560  RR(i,i) = 1.0;
1561  break;
1562 
1563  default:
1564  if ( nFR > 0 )
1565  {
1566  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
1567 
1568  /* get H */
1569  for ( j=0; j<nFR; ++j )
1570  DenseMatrix_getCol( _THIS->H, FR_idx[j], Bounds_getFree( &(_THIS->bounds) ), 1.0, &(_THIS->R[j*NVMAX]));
1571 
1572  /* R'*R = H */
1573  POTRF( "U", &_nFR, _THIS->R, &_nV, &info );
1574 
1575  /* <0 = invalid call, =0 ok, >0 not spd */
1576  if (info > 0) {
1577  if ( _THIS->R[0] < 0.0 )
1578  {
1579  /* Cholesky decomposition has tunneled a negative
1580  * diagonal element. */
1581  _THIS->options.epsRegularisation = qpOASES_getMin( -_THIS->R[0]+_THIS->options.epsRegularisation,qpOASES_getSqrt(qpOASES_getAbs(_THIS->options.epsRegularisation)) );
1582  }
1583 
1584  _THIS->hessianType = HST_SEMIDEF;
1585  return RET_HESSIAN_NOT_SPD;
1586  }
1587 
1588 
1589  /* zero first subdiagonal to make givens updates work */
1590  for (i=0;i<nFR-1;++i)
1591  RR(i+1,i) = 0.0;
1592 
1593  }
1594  }
1595 
1596  return SUCCESSFUL_RETURN;
1597 }
1598 
1599 
1600 
1601 /*
1602  * o b t a i n A u x i l i a r y W o r k i n g S e t
1603  */
1604 returnValue QProblemBCPY_obtainAuxiliaryWorkingSet( QProblem* _THIS, const real_t* const xOpt, const real_t* const yOpt,
1605  Bounds* const guessedBounds, Bounds* auxiliaryBounds
1606  )
1607 {
1608  int i = 0;
1609  int nV = QProblem_getNV( _THIS );
1610 
1611 
1612  /* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
1613  if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
1614  return THROWERROR( RET_INVALID_ARGUMENTS );
1615 
1616  /* 2) Setup working set for auxiliary initial QP. */
1617  if ( guessedBounds != 0 )
1618  {
1619  /* If an initial working set is specific, use it!
1620  * Moreover, add all implictly fixed variables if specified. */
1621  for( i=0; i<nV; ++i )
1622  {
1623  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
1624  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
1625  {
1626  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1627  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1628  }
1629  else
1630  #endif
1631  {
1632  if ( Bounds_setupBound( auxiliaryBounds,i,Bounds_getStatus( guessedBounds,i ) ) != SUCCESSFUL_RETURN )
1633  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1634  }
1635  }
1636  }
1637  else /* No initial working set specified. */
1638  {
1639  if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
1640  {
1641  /* Obtain initial working set by "clipping". */
1642  for( i=0; i<nV; ++i )
1643  {
1644  if ( xOpt[i] <= _THIS->lb[i] + _THIS->options.boundTolerance )
1645  {
1646  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1647  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1648  continue;
1649  }
1650 
1651  if ( xOpt[i] >= _THIS->ub[i] - _THIS->options.boundTolerance )
1652  {
1653  if ( Bounds_setupBound( auxiliaryBounds,i,ST_UPPER ) != SUCCESSFUL_RETURN )
1654  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1655  continue;
1656  }
1657 
1658  /* Moreover, add all implictly fixed variables if specified. */
1659  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
1660  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
1661  {
1662  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1663  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1664  }
1665  else
1666  #endif
1667  {
1668  if ( Bounds_setupBound( auxiliaryBounds,i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
1669  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1670  }
1671  }
1672  }
1673 
1674  if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
1675  {
1676  /* Obtain initial working set in accordance to sign of dual solution vector. */
1677  for( i=0; i<nV; ++i )
1678  {
1679  if ( yOpt[i] > QPOASES_EPS )
1680  {
1681  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1682  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1683  continue;
1684  }
1685 
1686  if ( yOpt[i] < -QPOASES_EPS )
1687  {
1688  if ( Bounds_setupBound( auxiliaryBounds,i,ST_UPPER ) != SUCCESSFUL_RETURN )
1689  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1690  continue;
1691  }
1692 
1693  /* Moreover, add all implictly fixed variables if specified. */
1694  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
1695  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
1696  {
1697  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1698  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1699  }
1700  else
1701  #endif
1702  {
1703  if ( Bounds_setupBound( auxiliaryBounds,i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
1704  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1705  }
1706  }
1707  }
1708 
1709  /* If xOpt and yOpt are null pointer and no initial working is specified,
1710  * start with empty working set (or implicitly fixed bounds only)
1711  * for auxiliary QP. */
1712  if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
1713  {
1714  for( i=0; i<nV; ++i )
1715  {
1716  switch( Bounds_getType( &(_THIS->bounds),i ) )
1717  {
1718  case ST_UNBOUNDED:
1719  if ( Bounds_setupBound( auxiliaryBounds,i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
1720  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1721  break;
1722 
1723  /* Only add all implictly fixed variables if specified. */
1724  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
1725  case ST_EQUALITY:
1726  if ( Bounds_setupBound( auxiliaryBounds,i,ST_LOWER ) != SUCCESSFUL_RETURN )
1727  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1728  break;
1729  #endif
1730 
1731  default:
1732  if ( Bounds_setupBound( auxiliaryBounds,i,_THIS->options.initialStatusBounds ) != SUCCESSFUL_RETURN )
1733  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
1734  break;
1735  }
1736  }
1737  }
1738  }
1739 
1740  return SUCCESSFUL_RETURN;
1741 }
1742 
1743 
1744 
1745 /*
1746  * b a c k s o l v e R
1747  */
1748 returnValue QProblem_backsolveR( QProblem* _THIS, const real_t* const b, BooleanType transposed,
1749  real_t* const a
1750  )
1751 {
1752  /* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */
1753  return QProblem_backsolveRrem( _THIS,b,transposed,BT_FALSE,a );
1754 }
1755 
1756 
1757 /*
1758  * b a c k s o l v e R
1759  */
1760 returnValue QProblem_backsolveRrem( QProblem* _THIS, const real_t* const b, BooleanType transposed,
1761  BooleanType removingBound,
1762  real_t* const a
1763  )
1764 {
1765  int i, j;
1766  int nR = QProblem_getNZ( _THIS );
1767 
1768  real_t sum;
1769 
1770  /* if backsolve is called while removing a bound, reduce nZ by one. */
1771  if ( removingBound == BT_TRUE )
1772  --nR;
1773 
1774  /* nothing to do */
1775  if ( nR <= 0 )
1776  return SUCCESSFUL_RETURN;
1777 
1778 
1779  /* Solve Ra = b, where R might be transposed. */
1780  if ( transposed == BT_FALSE )
1781  {
1782  /* solve Ra = b */
1783  for( i=(nR-1); i>=0; --i )
1784  {
1785  sum = b[i];
1786  for( j=(i+1); j<nR; ++j )
1787  sum -= RR(i,j) * a[j];
1788 
1789  if ( qpOASES_getAbs( RR(i,i) ) >= QPOASES_ZERO*qpOASES_getAbs( sum ) )
1790  a[i] = sum / RR(i,i);
1791  else
1792  return THROWERROR( RET_DIV_BY_ZERO );
1793  }
1794  }
1795  else
1796  {
1797  /* solve R^T*a = b */
1798  for( i=0; i<nR; ++i )
1799  {
1800  sum = b[i];
1801  for( j=0; j<i; ++j )
1802  sum -= RR(j,i) * a[j];
1803 
1804  if ( qpOASES_getAbs( RR(i,i) ) >= QPOASES_ZERO*qpOASES_getAbs( sum ) )
1805  a[i] = sum / RR(i,i);
1806  else
1807  return THROWERROR( RET_DIV_BY_ZERO );
1808  }
1809  }
1810 
1811  return SUCCESSFUL_RETURN;
1812 }
1813 
1814 
1815 /*
1816  * d e t e r m i n e D a t a S h i f t
1817  */
1818 returnValue QProblemBCPY_determineDataShift( QProblem* _THIS, const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
1819  real_t* const delta_g, real_t* const delta_lb, real_t* const delta_ub,
1820  BooleanType* Delta_bB_isZero
1821  )
1822 {
1823  int i, ii;
1824  int nV = QProblem_getNV( _THIS );
1825  int nFX = QProblem_getNFX( _THIS );
1826 
1827  int* FX_idx;
1828  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
1829 
1830 
1831  /* 1) Calculate shift directions. */
1832  for( i=0; i<nV; ++i )
1833  delta_g[i] = g_new[i] - _THIS->g[i];
1834 
1835  if ( lb_new != 0 )
1836  {
1837  for( i=0; i<nV; ++i )
1838  delta_lb[i] = lb_new[i] - _THIS->lb[i];
1839  }
1840  else
1841  {
1842  /* if no lower bounds exist, assume the new lower bounds to be -infinity */
1843  for( i=0; i<nV; ++i )
1844  delta_lb[i] = -QPOASES_INFTY - _THIS->lb[i];
1845  }
1846 
1847  if ( ub_new != 0 )
1848  {
1849  for( i=0; i<nV; ++i )
1850  delta_ub[i] = ub_new[i] - _THIS->ub[i];
1851  }
1852  else
1853  {
1854  /* if no upper bounds exist, assume the new upper bounds to be infinity */
1855  for( i=0; i<nV; ++i )
1856  delta_ub[i] = QPOASES_INFTY - _THIS->ub[i];
1857  }
1858 
1859  /* 2) Determine if active bounds are to be shifted. */
1860  *Delta_bB_isZero = BT_TRUE;
1861 
1862  for ( i=0; i<nFX; ++i )
1863  {
1864  ii = FX_idx[i];
1865 
1866  if ( ( qpOASES_getAbs( delta_lb[ii] ) > QPOASES_EPS ) || ( qpOASES_getAbs( delta_ub[ii] ) > QPOASES_EPS ) )
1867  {
1868  *Delta_bB_isZero = BT_FALSE;
1869  break;
1870  }
1871  }
1872 
1873  return SUCCESSFUL_RETURN;
1874 }
1875 
1876 
1877 
1878 /*
1879  * s e t u p Q P d a t a
1880  */
1881 returnValue QProblemBCPY_setupQPdataM( QProblem* _THIS, DenseMatrix *_H, const real_t* const _g,
1882  const real_t* const _lb, const real_t* const _ub
1883  )
1884 {
1885  if ( _H == 0 )
1886  return QProblemBCPY_setupQPdata( _THIS,(real_t*)0,_g,_lb,_ub );
1887  else
1888  return QProblemBCPY_setupQPdata( _THIS,DenseMatrix_getVal(_H),_g,_lb,_ub );
1889 }
1890 
1891 
1892 /*
1893  * s e t u p Q P d a t a
1894  */
1895 returnValue QProblemBCPY_setupQPdata( QProblem* _THIS, real_t* const _H, const real_t* const _g,
1896  const real_t* const _lb, const real_t* const _ub
1897  )
1898 {
1899  /* 1) Setup Hessian matrix. */
1900  QProblem_setH( _THIS,_H );
1901 
1902  /* 2) Setup gradient vector. */
1903  if ( _g == 0 )
1904  return THROWERROR( RET_INVALID_ARGUMENTS );
1905  else
1906  QProblem_setG( _THIS,_g );
1907 
1908  /* 3) Setup lower/upper bounds vector. */
1909  QProblem_setLB( _THIS,_lb );
1910  QProblem_setUB( _THIS,_ub );
1911 
1912  return SUCCESSFUL_RETURN;
1913 }
1914 
1915 
1916 /*
1917  * s e t u p Q P d a t a F r o m F i l e
1918  */
1919 returnValue QProblemBCPY_setupQPdataFromFile( QProblem* _THIS, const char* const H_file, const char* const g_file,
1920  const char* const lb_file, const char* const ub_file
1921  )
1922 {
1923  int i;
1924  int nV = QProblem_getNV( _THIS );
1925 
1926  returnValue returnvalue;
1927 
1928 
1929  /* 1) Load Hessian matrix from file. */
1930  myStatic real_t _H[NVMAX*NVMAX];
1931 
1932  if ( H_file != 0 )
1933  {
1934  returnvalue = qpOASES_readFromFileM( _H, nV,nV, H_file );
1935  if ( returnvalue != SUCCESSFUL_RETURN )
1936  return THROWERROR( returnvalue );
1937  QProblem_setH( _THIS,_H );
1938  }
1939  else
1940  {
1941  QProblem_setH( _THIS,(real_t*)0 );
1942  }
1943 
1944  /* 2) Load gradient vector from file. */
1945  if ( g_file == 0 )
1946  return THROWERROR( RET_INVALID_ARGUMENTS );
1947 
1948  returnvalue = qpOASES_readFromFileV( _THIS->g, nV, g_file );
1949  if ( returnvalue != SUCCESSFUL_RETURN )
1950  return THROWERROR( returnvalue );
1951 
1952  /* 3) Load lower bounds vector from file. */
1953  if ( lb_file != 0 )
1954  {
1955  returnvalue = qpOASES_readFromFileV( _THIS->lb, nV, lb_file );
1956  if ( returnvalue != SUCCESSFUL_RETURN )
1957  return THROWERROR( returnvalue );
1958  }
1959  else
1960  {
1961  /* if no lower bounds are specified, set them to -infinity */
1962  for( i=0; i<nV; ++i )
1963  _THIS->lb[i] = -QPOASES_INFTY;
1964  }
1965 
1966  /* 4) Load upper bounds vector from file. */
1967  if ( ub_file != 0 )
1968  {
1969  returnvalue = qpOASES_readFromFileV( _THIS->ub, nV, ub_file );
1970  if ( returnvalue != SUCCESSFUL_RETURN )
1971  return THROWERROR( returnvalue );
1972  }
1973  else
1974  {
1975  /* if no upper bounds are specified, set them to infinity */
1976  for( i=0; i<nV; ++i )
1977  _THIS->ub[i] = QPOASES_INFTY;
1978  }
1979 
1980  return SUCCESSFUL_RETURN;
1981 }
1982 
1983 
1984 /*
1985  * l o a d Q P v e c t o r s F r o m F i l e
1986  */
1987 returnValue QProblemBCPY_loadQPvectorsFromFile( QProblem* _THIS, const char* const g_file, const char* const lb_file, const char* const ub_file,
1988  real_t* const g_new, real_t* const lb_new, real_t* const ub_new
1989  )
1990 {
1991  int nV = QProblem_getNV( _THIS );
1992 
1993  returnValue returnvalue;
1994 
1995 
1996  /* 1) Load gradient vector from file. */
1997  if ( ( g_file != 0 ) && ( g_new != 0 ) )
1998  {
1999  returnvalue = qpOASES_readFromFileV( g_new, nV, g_file );
2000  if ( returnvalue != SUCCESSFUL_RETURN )
2001  return THROWERROR( returnvalue );
2002  }
2003  else
2004  {
2005  /* At least gradient vector needs to be specified! */
2006  return THROWERROR( RET_INVALID_ARGUMENTS );
2007  }
2008 
2009  /* 2) Load lower bounds vector from file. */
2010  if ( lb_file != 0 )
2011  {
2012  if ( lb_new != 0 )
2013  {
2014  returnvalue = qpOASES_readFromFileV( lb_new, nV, lb_file );
2015  if ( returnvalue != SUCCESSFUL_RETURN )
2016  return THROWERROR( returnvalue );
2017  }
2018  else
2019  {
2020  /* If filename is given, storage must be provided! */
2021  return THROWERROR( RET_INVALID_ARGUMENTS );
2022  }
2023  }
2024 
2025  /* 3) Load upper bounds vector from file. */
2026  if ( ub_file != 0 )
2027  {
2028  if ( ub_new != 0 )
2029  {
2030  returnvalue = qpOASES_readFromFileV( ub_new, nV, ub_file );
2031  if ( returnvalue != SUCCESSFUL_RETURN )
2032  return THROWERROR( returnvalue );
2033  }
2034  else
2035  {
2036  /* If filename is given, storage must be provided! */
2037  return THROWERROR( RET_INVALID_ARGUMENTS );
2038  }
2039  }
2040 
2041  return SUCCESSFUL_RETURN;
2042 }
2043 
2044 
2045 /*
2046  * s e t I n f e a s i b i l i t y F l a g
2047  */
2048 returnValue QProblem_setInfeasibilityFlag( QProblem* _THIS,
2049  returnValue returnvalue, BooleanType doThrowError
2050  )
2051 {
2052  _THIS->infeasible = BT_TRUE;
2053 
2054  if ( _THIS->options.enableFarBounds == BT_FALSE )
2055  THROWERROR( returnvalue );
2056 
2057  return returnvalue;
2058 }
2059 
2060 
2061 /*
2062  * i s C P U t i m e L i m i t E x c e e d e d
2063  */
2064 BooleanType QProblem_isCPUtimeLimitExceeded( QProblem* _THIS, const real_t* const cputime,
2065  real_t starttime,
2066  int nWSR
2067  )
2068 {
2069  real_t elapsedTime, timePerIteration;
2070 
2071  /* Always perform next QP iteration if no CPU time limit is given. */
2072  if ( cputime == 0 )
2073  return BT_FALSE;
2074 
2075  /* Always perform first QP iteration. */
2076  if ( nWSR <= 0 )
2077  return BT_FALSE;
2078 
2079  elapsedTime = qpOASES_getCPUtime( ) - starttime;
2080  timePerIteration = elapsedTime / ((real_t) nWSR);
2081 
2082  /* Determine if next QP iteration exceed CPU time limit
2083  * considering the (current) average CPU time per iteration. */
2084  if ( ( elapsedTime + timePerIteration*1.25 ) <= ( *cputime ) )
2085  return BT_FALSE;
2086  else
2087  return BT_TRUE;
2088 }
2089 
2090 
2091 /*
2092  * r e g u l a r i s e H e s s i a n
2093  */
2094 returnValue QProblem_regulariseHessian( QProblem* _THIS )
2095 {
2096  /* Do nothing if Hessian regularisation is disbaled! */
2097  if ( _THIS->options.enableRegularisation == BT_FALSE )
2098  return SUCCESSFUL_RETURN;
2099 
2100  /* Regularisation of identity Hessian not possible. */
2101  if ( _THIS->hessianType == HST_IDENTITY )
2102  return THROWERROR( RET_CANNOT_REGULARISE_IDENTITY );
2103 
2104  /* Determine regularisation parameter. */
2105  if ( QProblem_usingRegularisation( _THIS ) == BT_TRUE )
2106  return SUCCESSFUL_RETURN; /*THROWERROR( RET_HESSIAN_ALREADY_REGULARISED );*/
2107  else
2108  {
2109  /* Regularisation of zero Hessian is done implicitly. */
2110  if ( _THIS->hessianType == HST_ZERO )
2111  {
2112  _THIS->regVal = qpOASES_getNorm( _THIS->g,QProblem_getNV( _THIS ),2 ) * _THIS->options.epsRegularisation;
2113  }
2114  else
2115  {
2116  _THIS->regVal = DenseMatrix_getNorm( _THIS->H,2 ) * _THIS->options.epsRegularisation;
2117 
2118  if ( DenseMatrix_addToDiag( _THIS->H,_THIS->regVal ) == RET_NO_DIAGONAL_AVAILABLE )
2119  return THROWERROR( RET_CANNOT_REGULARISE_SPARSE );
2120  }
2121 
2122  THROWINFO( RET_USING_REGULARISATION );
2123  }
2124 
2125  return SUCCESSFUL_RETURN;
2126 }
2127 
2128 
2129 
2130 /*
2131  * p e r f o r m R a t i o T e s t
2132  */
2133 returnValue QProblem_performRatioTestB( QProblem* _THIS,
2134  int nIdx,
2135  const int* const idxList,
2136  Bounds* const subjectTo,
2137  const real_t* const num,
2138  const real_t* const den,
2139  real_t epsNum,
2140  real_t epsDen,
2141  real_t* t,
2142  int* BC_idx
2143  )
2144 {
2145  int i, ii;
2146 
2147  *BC_idx = -1;
2148 
2149  for( i=0; i<nIdx; ++i )
2150  {
2151  ii = idxList[i];
2152 
2153  if ( Bounds_getType( subjectTo,ii ) != ST_EQUALITY )
2154  {
2155  if ( ( Bounds_getStatus( subjectTo,ii ) == ST_LOWER ) || ( Bounds_getStatus( subjectTo,ii ) == ST_INACTIVE ) )
2156  {
2157  if ( QProblem_isBlocking( _THIS,num[i],den[i],epsNum,epsDen,t ) == BT_TRUE )
2158  {
2159  *t = num[i] / den[i];
2160  *BC_idx = ii;
2161  }
2162  }
2163  else
2164  if ( Bounds_getStatus( subjectTo,ii ) == ST_UPPER )
2165  {
2166  if ( QProblem_isBlocking( _THIS,-num[i],-den[i],epsNum,epsDen,t ) == BT_TRUE )
2167  {
2168  *t = num[i] / den[i];
2169  *BC_idx = ii;
2170  }
2171  }
2172  }
2173  }
2174 
2175  return SUCCESSFUL_RETURN;
2176 }
2177 
2178 
2179 
2180 /*
2181  * r e l a t i v e H o m o t o p y L e n g t h
2182  */
2183 real_t QProblemBCPY_getRelativeHomotopyLength( QProblem* _THIS,
2184  const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new
2185  )
2186 {
2187  int i;
2188  int nV = QProblem_getNV( _THIS );
2189  real_t d, s, len = 0.0;
2190 
2191  /* gradient */
2192  for (i = 0; i < nV; i++)
2193  {
2194  s = qpOASES_getAbs(g_new[i]);
2195  if (s < 1.0) s = 1.0;
2196  d = qpOASES_getAbs(g_new[i] - _THIS->g[i]) / s;
2197  if (d > len) len = d;
2198  }
2199 
2200  /* lower bounds */
2201  if ( lb_new != 0 )
2202  {
2203  for (i = 0; i < nV; i++)
2204  {
2205  s = qpOASES_getAbs(lb_new[i]);
2206  if (s < 1.0) s = 1.0;
2207  d = qpOASES_getAbs(lb_new[i] - _THIS->lb[i]) / s;
2208  if (d > len) len = d;
2209  }
2210  }
2211 
2212  /* upper bounds */
2213  if ( ub_new != 0 )
2214  {
2215  for (i = 0; i < nV; i++)
2216  {
2217  s = qpOASES_getAbs(ub_new[i]);
2218  if (s < 1.0) s = 1.0;
2219  d = qpOASES_getAbs(ub_new[i] - _THIS->ub[i]) / s;
2220  if (d > len) len = d;
2221  }
2222  }
2223 
2224  return len;
2225 }
2226 
2227 
2228 
2229 /*
2230  * s o l v e I n i t i a l Q P
2231  */
2232 returnValue QProblem_solveInitialQP( QProblem* _THIS,
2233  const real_t* const xOpt, const real_t* const yOpt,
2234  Bounds* const guessedBounds, Constraints* const guessedConstraints,
2235  const real_t* const _R,
2236  int* nWSR, real_t* const cputime
2237  )
2238 {
2239  int i,j;
2240 
2241  /* some definitions */
2242  int nV = QProblem_getNV( _THIS );
2243  int nC = QProblem_getNC( _THIS );
2244 
2245  myStatic Bounds auxiliaryBounds;
2246  myStatic Constraints auxiliaryConstraints;
2247 
2248  myStatic real_t g_original[NVMAX];
2249  myStatic real_t lb_original[NVMAX];
2250  myStatic real_t ub_original[NVMAX];
2251  myStatic real_t lbA_original[NCMAX];
2252  myStatic real_t ubA_original[NCMAX];
2253 
2254  returnValue returnvalue;
2255 
2256 
2257  /* start runtime measurement */
2258  real_t starttime = 0.0;
2259  if ( cputime != 0 )
2260  starttime = qpOASES_getCPUtime( );
2261 
2262 
2263  /*DenseMatrix_print( _THIS->H );
2264  DenseMatrix_print( _THIS->A );
2265 
2266  qpOASES_printV( _THIS->g,nV );
2267  qpOASES_printV( _THIS->lb,nV );
2268  qpOASES_printV( _THIS->ub,nV );
2269  qpOASES_printV( _THIS->lbA,nC );
2270  qpOASES_printV( _THIS->ubA,nC );*/
2271 
2272 
2273  _THIS->status = QPS_NOTINITIALISED;
2274 
2275  BoundsCON( &auxiliaryBounds,nV );
2276  ConstraintsCON( &auxiliaryConstraints,nC );
2277 
2278  /* I) ANALYSE QP DATA: */
2279  /* 1) Check if Hessian happens to be the identity matrix. */
2280  if ( QProblem_determineHessianType( _THIS ) != SUCCESSFUL_RETURN )
2281  return THROWERROR( RET_INIT_FAILED );
2282 
2283  /* 2) Setup type of bounds and constraints (i.e. unbounded, implicitly fixed etc.). */
2284  if ( QProblem_setupSubjectToType( _THIS ) != SUCCESSFUL_RETURN )
2285  return THROWERROR( RET_INIT_FAILED );
2286 
2287  _THIS->status = QPS_PREPARINGAUXILIARYQP;
2288 
2289 
2290  /* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
2291  /* 1) Setup bounds and constraints data structure. */
2292  if ( Bounds_setupAllFree( &(_THIS->bounds) ) != SUCCESSFUL_RETURN )
2293  return THROWERROR( RET_INIT_FAILED );
2294 
2295  if ( Constraints_setupAllInactive( &(_THIS->constraints) ) != SUCCESSFUL_RETURN )
2296  return THROWERROR( RET_INIT_FAILED );
2297 
2298  /* 2) Setup optimal primal/dual solution for auxiliary QP. */
2299  if ( QProblem_setupAuxiliaryQPsolution( _THIS,xOpt,yOpt ) != SUCCESSFUL_RETURN )
2300  return THROWERROR( RET_INIT_FAILED );
2301 
2302  /* 3) Obtain linear independent working set for auxiliary QP. */
2303  if ( QProblem_obtainAuxiliaryWorkingSet( _THIS,xOpt,yOpt,guessedBounds,guessedConstraints,
2304  &auxiliaryBounds,&auxiliaryConstraints ) != SUCCESSFUL_RETURN )
2305  return THROWERROR( RET_INIT_FAILED );
2306 
2307  /* 4) Setup working set of auxiliary QP and setup matrix factorisations. */
2308  /* a) Regularise Hessian if necessary. */
2309  if ( ( _THIS->hessianType == HST_ZERO ) || ( _THIS->hessianType == HST_SEMIDEF ) )
2310  {
2311  if ( QProblem_regulariseHessian( _THIS ) != SUCCESSFUL_RETURN )
2312  return THROWERROR( RET_INIT_FAILED_REGULARISATION );
2313  }
2314 
2315  /* b) TQ factorisation. */
2316  if ( QProblem_setupTQfactorisation( _THIS ) != SUCCESSFUL_RETURN )
2317  return THROWERROR( RET_INIT_FAILED_TQ );
2318 
2319  /* c) Working set of auxiliary QP. */
2320  if ( QProblem_setupAuxiliaryWorkingSet( _THIS,&auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
2321  return THROWERROR( RET_INIT_FAILED );
2322 
2323  /* d) Copy external Cholesky factor if provided */
2324  _THIS->haveCholesky = BT_FALSE;
2325 
2326  if ( _R != 0 )
2327  {
2328  if ( _THIS->options.initialStatusBounds != ST_INACTIVE )
2329  {
2330  THROWWARNING( RET_NO_CHOLESKY_WITH_INITIAL_GUESS );
2331  }
2332  else
2333  {
2334  if ( _R == _THIS->R )
2335  {
2336  /* Cholesky factor read from file and already loaded into R. */
2337  _THIS->haveCholesky = BT_TRUE;
2338  }
2339  else if ( ( xOpt == 0 ) && ( yOpt == 0 ) && ( guessedBounds == 0 ) && ( guessedConstraints == 0 ) )
2340  {
2341  for( i=0; i<nV; ++i )
2342  for( j=i; j<nV; ++j )
2343  RR(i,j) = _R[i*nV+j];
2344  _THIS->haveCholesky = BT_TRUE;
2345  }
2346  else
2347  {
2348  THROWWARNING( RET_NO_CHOLESKY_WITH_INITIAL_GUESS );
2349  }
2350  }
2351  }
2352 
2353  /* 5) Store original QP formulation... */
2354  for( i=0; i<nV; ++i )
2355  g_original[i] = _THIS->g[i];
2356  for( i=0; i<nV; ++i )
2357  lb_original[i] = _THIS->lb[i];
2358  for( i=0; i<nV; ++i )
2359  ub_original[i] = _THIS->ub[i];
2360 
2361  for( i=0; i<nC; ++i )
2362  lbA_original[i] = _THIS->lbA[i];
2363  for( i=0; i<nC; ++i )
2364  ubA_original[i] = _THIS->ubA[i];
2365 
2366  /* ... and setup QP data of an auxiliary QP having an optimal solution
2367  * as specified by the user (or xOpt = yOpt = 0, by default). */
2368  if ( QProblem_setupAuxiliaryQPgradient( _THIS ) != SUCCESSFUL_RETURN )
2369  return THROWERROR( RET_INIT_FAILED );
2370 
2371  if ( QProblem_setupAuxiliaryQPbounds( _THIS,&auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
2372  return THROWERROR( RET_INIT_FAILED );
2373 
2374  _THIS->status = QPS_AUXILIARYQPSOLVED;
2375 
2376 
2377  if ( _THIS->options.enableRamping == BT_TRUE )
2378  QProblem_performRamping( _THIS );
2379 
2380 
2381  /* III) SOLVE ACTUAL INITIAL QP: */
2382  /* Allow only remaining CPU time for usual hotstart. */
2383  if ( cputime != 0 )
2384  *cputime -= qpOASES_getCPUtime( ) - starttime;
2385 
2386  /* Use hotstart method to find the solution of the original initial QP,... */
2387  returnvalue = QProblem_hotstart( _THIS,g_original,lb_original,ub_original,lbA_original,ubA_original, nWSR,cputime );
2388 
2389  /* ... check for infeasibility and unboundedness... */
2390  if ( QProblem_isInfeasible( _THIS ) == BT_TRUE )
2391  return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
2392 
2393  if ( QProblem_isUnbounded( _THIS ) == BT_TRUE )
2394  return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
2395 
2396  /* ... and internal errors. */
2397  if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
2398  return THROWERROR( RET_INIT_FAILED_HOTSTART );
2399 
2400 
2401  /* stop runtime measurement */
2402  if ( cputime != 0 )
2403  *cputime = qpOASES_getCPUtime( ) - starttime;
2404 
2405  THROWINFO( RET_INIT_SUCCESSFUL );
2406 
2407  return returnvalue;
2408 }
2409 
2410 
2411 /*
2412  * s o l v e Q P
2413  */
2414 returnValue QProblem_solveQP( QProblem* _THIS,
2415  const real_t* const g_new,
2416  const real_t* const lb_new, const real_t* const ub_new,
2417  const real_t* const lbA_new, const real_t* const ubA_new,
2418  int* nWSR, real_t* const cputime, int nWSRperformed,
2419  BooleanType isFirstCall
2420  )
2421 {
2422  int iter;
2423 
2424  /* I) PREPARATIONS */
2425  /* 1) Allocate delta vectors of gradient and (constraints') bounds,
2426  * index arrays and step direction arrays. */
2427 
2428  myStatic real_t delta_xFR[NVMAX];
2429  myStatic real_t delta_xFX[NVMAX];
2430  myStatic real_t delta_yAC[NCMAX];
2431  myStatic real_t delta_yFX[NVMAX];
2432 
2433  myStatic real_t delta_g[NVMAX];
2434  myStatic real_t delta_lb[NVMAX];
2435  myStatic real_t delta_ub[NVMAX];
2436  myStatic real_t delta_lbA[NCMAX];
2437  myStatic real_t delta_ubA[NCMAX];
2438 
2439  returnValue returnvalue;
2440  BooleanType Delta_bC_isZero, Delta_bB_isZero;
2441 
2442  int BC_idx;
2443  SubjectToStatus BC_status;
2444  BooleanType BC_isBound;
2445 
2446  real_t homotopyLength;
2447 
2448  #ifndef __SUPPRESSANYOUTPUT__
2449  myStatic char messageString[QPOASES_MAX_STRING_LENGTH];
2450  #endif
2451 
2452  /* start runtime measurement */
2453  real_t starttime = 0.0;
2454  if ( cputime != 0 )
2455  starttime = qpOASES_getCPUtime( );
2456 
2457 
2458  /* consistency check */
2459  if ( ( QProblem_getStatus( _THIS ) == QPS_NOTINITIALISED ) ||
2460  ( QProblem_getStatus( _THIS ) == QPS_PREPARINGAUXILIARYQP ) ||
2461  ( QProblem_getStatus( _THIS ) == QPS_PERFORMINGHOMOTOPY ) )
2462  {
2463  return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
2464  }
2465 
2466 
2467  /* 2) Update type of bounds and constraints, e.g.
2468  * a former equality constraint might have become a normal one etc. */
2469 
2470 /* (ckirches) disabled _THIS, as inactive but tight bounds may become inactive equalities
2471  which would then never become active again!*/
2472 /*
2473  if ( setupSubjectToType( _THIS,lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
2474  return THROWERROR( RET_HOTSTART_FAILED );
2475 */
2476  /* 3) Reset status flags. */
2477  _THIS->infeasible = BT_FALSE;
2478  _THIS->unbounded = BT_FALSE;
2479 
2480 
2481  /* II) MAIN HOMOTOPY LOOP */
2482  for( iter=nWSRperformed; iter<*nWSR; ++iter )
2483  {
2484  _THIS->tabularOutput.idxAddB = _THIS->tabularOutput.idxRemB = _THIS->tabularOutput.idxAddC = _THIS->tabularOutput.idxRemC = -1;
2485  _THIS->tabularOutput.excAddB = _THIS->tabularOutput.excRemB = _THIS->tabularOutput.excAddC = _THIS->tabularOutput.excRemC = 0;
2486 
2487  if ( QProblem_isCPUtimeLimitExceeded( _THIS,cputime,starttime,iter-nWSRperformed ) == BT_TRUE )
2488  {
2489  /* If CPU time limit is exceeded, stop homotopy loop immediately!
2490  * Assign number of working set recalculations (runtime measurement is stopped later). */
2491  *nWSR = iter;
2492  break;
2493  }
2494 
2495  /*qpOASES_printV( _THIS->x,QProblem_getNV(_THIS) );*/
2496 
2497  _THIS->status = QPS_PERFORMINGHOMOTOPY;
2498 
2499  #ifndef __SUPPRESSANYOUTPUT__
2500  if ( isFirstCall == BT_TRUE )
2501  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"%d ...",iter );
2502  else
2503  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"%d* ...",iter );
2504  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_ITERATION_STARTED,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
2505  #endif
2506 
2507 
2508  /* 2) Determination of shift direction of the gradient and the (constraints') bounds. */
2509  returnvalue = QProblem_determineDataShift( _THIS,g_new,lbA_new,ubA_new,lb_new,ub_new,
2510  delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
2511  &Delta_bC_isZero,&Delta_bB_isZero
2512  );
2513  if ( returnvalue != SUCCESSFUL_RETURN )
2514  {
2515  /* Assign number of working set recalculations and stop runtime measurement. */
2516  *nWSR = iter;
2517  if ( cputime != 0 )
2518  *cputime = qpOASES_getCPUtime( ) - starttime;
2519 
2520  THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
2521  return returnvalue;
2522  }
2523 
2524  /* 3) Determination of step direction of X and Y. */
2525  returnvalue = QProblem_determineStepDirection( _THIS,delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
2526  Delta_bC_isZero, Delta_bB_isZero,
2527  delta_xFX,delta_xFR,delta_yAC,delta_yFX
2528  );
2529  if ( returnvalue != SUCCESSFUL_RETURN )
2530  {
2531  /* Assign number of working set recalculations and stop runtime measurement. */
2532  *nWSR = iter;
2533  if ( cputime != 0 )
2534  *cputime = qpOASES_getCPUtime( ) - starttime;
2535 
2536  THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
2537  return returnvalue;
2538  }
2539 
2540  /* 4) Determination of step length TAU.
2541  * This step along the homotopy path is also taken (without changing working set). */
2542  returnvalue = QProblem_performStep( _THIS,delta_g, delta_lbA,delta_ubA,delta_lb,delta_ub,
2543  delta_xFX,delta_xFR,delta_yAC,delta_yFX,
2544  &BC_idx,&BC_status,&BC_isBound
2545  );
2546  if ( returnvalue != SUCCESSFUL_RETURN )
2547  {
2548  /* Assign number of working set recalculations and stop runtime measurement. */
2549  *nWSR = iter;
2550  if ( cputime != 0 )
2551  *cputime = qpOASES_getCPUtime( ) - starttime;
2552 
2553  THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
2554  return returnvalue;
2555  }
2556 
2557  /* 5) Termination criterion. */
2558  homotopyLength = QProblem_getRelativeHomotopyLength( _THIS,g_new,lb_new,ub_new,lbA_new,ubA_new );
2559  /* fprintf( stdFile, "*homotopyLength* = %.3e\n",homotopyLength ); */
2560 
2561  if ( homotopyLength <= _THIS->options.terminationTolerance )
2562  {
2563  _THIS->status = QPS_SOLVED;
2564 
2565  THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
2566 
2567  if ( QProblem_printIteration( _THIS,iter,BC_idx,BC_status,BC_isBound,homotopyLength,isFirstCall ) != SUCCESSFUL_RETURN )
2568  THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass _THIS as return value! */
2569 
2570  *nWSR = iter;
2571  if ( cputime != 0 )
2572  *cputime = qpOASES_getCPUtime( ) - starttime;
2573 
2574  return SUCCESSFUL_RETURN;
2575  }
2576 
2577 
2578  /* 6) Change active set. */
2579  returnvalue = QProblem_changeActiveSet( _THIS,BC_idx,BC_status,BC_isBound );
2580  if ( returnvalue != SUCCESSFUL_RETURN )
2581  {
2582  /* Assign number of working set recalculations and stop runtime measurement. */
2583  *nWSR = iter;
2584  if ( cputime != 0 )
2585  *cputime = qpOASES_getCPUtime( ) - starttime;
2586 
2587  /* Checks for infeasibility... */
2588  if ( QProblem_isInfeasible( _THIS ) == BT_TRUE )
2589  {
2590  _THIS->status = QPS_HOMOTOPYQPSOLVED;
2591  return QProblem_setInfeasibilityFlag( _THIS,RET_HOTSTART_STOPPED_INFEASIBILITY,BT_FALSE );
2592  }
2593 
2594  /* ...unboundedness... */
2595  if ( QProblem_isUnbounded( _THIS ) == BT_TRUE ) /* not necessary since objective function convex! */
2596  return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
2597 
2598  /* ... and throw unspecific error otherwise */
2599  THROWERROR( RET_HOMOTOPY_STEP_FAILED );
2600  return returnvalue;
2601  }
2602 
2603  /* 6a) Possibly refactorise projected Hessian from scratch. */
2604  if (_THIS->options.enableCholeskyRefactorisation > 0 && iter % _THIS->options.enableCholeskyRefactorisation == 0)
2605  {
2606  returnvalue = QProblem_computeProjectedCholesky( _THIS );
2607  if (returnvalue != SUCCESSFUL_RETURN)
2608  return returnvalue;
2609  }
2610 
2611  /* 7) Output information of successful QP iteration. */
2612  _THIS->status = QPS_HOMOTOPYQPSOLVED;
2613 
2614  if ( QProblem_printIteration( _THIS,iter,BC_idx,BC_status,BC_isBound,homotopyLength,isFirstCall ) != SUCCESSFUL_RETURN )
2615  THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass _THIS as return value! */
2616 
2617  /* 8) Perform Ramping Strategy on zero homotopy step or drift correction (if desired). */
2618  if (BC_status != ST_UNDEFINED)
2619  {
2620  if ( ( _THIS->tau <= QPOASES_EPS ) && ( _THIS->options.enableRamping == BT_TRUE ) )
2621  QProblem_performRamping( _THIS );
2622  else
2623  if ( (_THIS->options.enableDriftCorrection > 0)
2624  && ((iter+1) % _THIS->options.enableDriftCorrection == 0) )
2625  QProblem_performDriftCorrection( _THIS ); /* always returns SUCCESSFUL_RETURN */
2626  }
2627  }
2628 
2629  /* stop runtime measurement */
2630  if ( cputime != 0 )
2631  *cputime = qpOASES_getCPUtime( ) - starttime;
2632 
2633 
2634  /* if program gets to here, output information that QP could not be solved
2635  * within the given maximum numbers of working set changes */
2636  if ( _THIS->options.printLevel == PL_HIGH )
2637  {
2638  #ifndef __SUPPRESSANYOUTPUT__
2639  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"(nWSR = %d)",iter );
2640  return MessageHandling_throwWarning( qpOASES_getGlobalMessageHandler(),RET_MAX_NWSR_REACHED,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
2641  #else
2642  return RET_MAX_NWSR_REACHED;
2643  #endif
2644  }
2645  else
2646  {
2647  return RET_MAX_NWSR_REACHED;
2648  }
2649 }
2650 
2651 
2652 /*
2653  * s o l v e R e g u l a r i s e d Q P
2654  */
2655 returnValue QProblem_solveRegularisedQP( QProblem* _THIS,
2656  const real_t* const g_new,
2657  const real_t* const lb_new, const real_t* const ub_new,
2658  const real_t* const lbA_new, const real_t* const ubA_new,
2659  int* nWSR, real_t* const cputime, int nWSRperformed,
2660  BooleanType isFirstCall
2661  )
2662 {
2663  int i, step;
2664  int nV = QProblem_getNV( _THIS );
2665 
2666  returnValue returnvalue;
2667 
2668  int nWSR_max = *nWSR;
2669  int nWSR_total = nWSRperformed;
2670 
2671  real_t cputime_total = 0.0;
2672  real_t cputime_cur = 0.0;
2673 
2674  myStatic real_t gMod[NVMAX];
2675 
2676  /* Perform normal QP solution if QP has not been regularised. */
2677  if ( QProblem_usingRegularisation( _THIS ) == BT_FALSE )
2678  return QProblem_solveQP( _THIS,g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,nWSRperformed,isFirstCall );
2679 
2680 
2681  /* I) SOLVE USUAL REGULARISED QP */
2682  if ( cputime == 0 )
2683  {
2684  returnvalue = QProblem_solveQP( _THIS,g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0,nWSRperformed,isFirstCall );
2685  }
2686  else
2687  {
2688  cputime_cur = *cputime;
2689  returnvalue = QProblem_solveQP( _THIS,g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,&cputime_cur,nWSRperformed,isFirstCall );
2690  }
2691  nWSR_total = *nWSR;
2692  cputime_total += cputime_cur;
2693  isFirstCall = BT_FALSE;
2694 
2695  /* Only continue if QP solution has been successful. */
2696  if ( returnvalue != SUCCESSFUL_RETURN )
2697  {
2698  if ( cputime != 0 )
2699  *cputime = cputime_total;
2700 
2701  if ( returnvalue == RET_MAX_NWSR_REACHED )
2702  THROWWARNING( RET_NO_REGSTEP_NWSR );
2703 
2704  return returnvalue;
2705  }
2706 
2707 
2708  /* II) PERFORM SUCCESSIVE REGULARISATION STEPS */
2709  for( step=0; step<_THIS->options.numRegularisationSteps; ++step )
2710  {
2711  /* 1) Modify gradient: gMod = g - eps*xOpt
2712  * (assuming regularisation matrix to be regVal*Id). */
2713  for( i=0; i<nV; ++i )
2714  gMod[i] = g_new[i] - _THIS->regVal*_THIS->x[i];
2715 
2716  /* 2) Solve regularised QP with modified gradient allowing
2717  * only as many working set recalculations and CPU time
2718  * as have been left from previous QP solutions. */
2719  *nWSR = nWSR_max;
2720 
2721  if ( cputime == 0 )
2722  {
2723  returnvalue = QProblem_solveQP( _THIS,gMod,lb_new,ub_new,lbA_new,ubA_new, nWSR,0,nWSR_total,isFirstCall );
2724  }
2725  else
2726  {
2727  cputime_cur = *cputime - cputime_total;
2728  returnvalue = QProblem_solveQP( _THIS,gMod,lb_new,ub_new,lbA_new,ubA_new, nWSR,&cputime_cur,nWSR_total,isFirstCall );
2729  }
2730 
2731  nWSR_total = *nWSR;
2732  cputime_total += cputime_cur;
2733 
2734  /* Only continue if QP solution has been successful. */
2735  if ( returnvalue != SUCCESSFUL_RETURN )
2736  {
2737  if ( cputime != 0 )
2738  *cputime = cputime_total;
2739 
2740  if ( returnvalue == RET_MAX_NWSR_REACHED )
2741  THROWWARNING( RET_FEWER_REGSTEPS_NWSR );
2742 
2743  return returnvalue;
2744  }
2745  }
2746 
2747  for( i=0; i<nV; ++i )
2748  _THIS->g[i] = g_new[i];
2749 
2750  if ( cputime != 0 )
2751  *cputime = cputime_total;
2752 
2753  return SUCCESSFUL_RETURN;
2754 }
2755 
2756 
2757 /*
2758  * s e t u p S u b j e c t T o T y p e
2759  */
2760 returnValue QProblem_setupSubjectToType( QProblem* _THIS )
2761 {
2762  return QProblem_setupSubjectToTypeNew( _THIS,_THIS->lb,_THIS->ub,_THIS->lbA,_THIS->ubA );
2763 }
2764 
2765 
2766 /*
2767  * s e t u p S u b j e c t T o T y p e
2768  */
2769 returnValue QProblem_setupSubjectToTypeNew( QProblem* _THIS,
2770  const real_t* const lb_new, const real_t* const ub_new,
2771  const real_t* const lbA_new, const real_t* const ubA_new
2772  )
2773 {
2774  int i;
2775  int nV = QProblem_getNV( _THIS );
2776  int nC = QProblem_getNC( _THIS );
2777 
2778 
2779  /* I) SETUP SUBJECTTOTYPE FOR BOUNDS */
2780  /* 1) Check if lower bounds are present. */
2781  Bounds_setNoLower( &(_THIS->bounds),BT_TRUE );
2782  if ( lb_new != 0 )
2783  {
2784  for( i=0; i<nV; ++i )
2785  {
2786  if ( lb_new[i] > -QPOASES_INFTY )
2787  {
2788  Bounds_setNoLower( &(_THIS->bounds),BT_FALSE );
2789  break;
2790  }
2791  }
2792  }
2793 
2794  /* 2) Check if upper bounds are present. */
2795  Bounds_setNoUpper( &(_THIS->bounds),BT_TRUE );
2796  if ( ub_new != 0 )
2797  {
2798  for( i=0; i<nV; ++i )
2799  {
2800  if ( ub_new[i] < QPOASES_INFTY )
2801  {
2802  Bounds_setNoUpper( &(_THIS->bounds),BT_FALSE );
2803  break;
2804  }
2805  }
2806  }
2807 
2808  /* 3) Determine implicitly fixed and unbounded variables. */
2809  if ( ( lb_new != 0 ) && ( ub_new != 0 ) )
2810  {
2811  for( i=0; i<nV; ++i )
2812  {
2813  if ( ( lb_new[i] < -QPOASES_INFTY + _THIS->options.boundTolerance ) && ( ub_new[i] > QPOASES_INFTY - _THIS->options.boundTolerance )
2814  && (_THIS->options.enableFarBounds == BT_FALSE))
2815  {
2816  Bounds_setType( &(_THIS->bounds),i,ST_UNBOUNDED );
2817  }
2818  else
2819  {
2820  if ( _THIS->options.enableEqualities
2821  && _THIS->lb[i] > _THIS->ub[i] - _THIS->options.boundTolerance
2822  && lb_new[i] > ub_new[i] - _THIS->options.boundTolerance )
2823  Bounds_setType( &(_THIS->bounds),i,ST_EQUALITY );
2824  else
2825  Bounds_setType( &(_THIS->bounds),i,ST_BOUNDED );
2826  }
2827  }
2828  }
2829  else
2830  {
2831  if ( ( lb_new == 0 ) && ( ub_new == 0 ) )
2832  {
2833  for( i=0; i<nV; ++i )
2834  Bounds_setType( &(_THIS->bounds),i,ST_UNBOUNDED );
2835  }
2836  else
2837  {
2838  for( i=0; i<nV; ++i )
2839  Bounds_setType( &(_THIS->bounds),i,ST_BOUNDED );
2840  }
2841  }
2842 
2843 
2844  /* II) SETUP SUBJECTTOTYPE FOR CONSTRAINTS */
2845  /* 1) Check if lower constraints' bounds are present. */
2846  Constraints_setNoLower( &(_THIS->constraints),BT_TRUE );
2847  if ( lbA_new != 0 )
2848  {
2849  for( i=0; i<nC; ++i )
2850  {
2851  if ( lbA_new[i] > -QPOASES_INFTY )
2852  {
2853  Constraints_setNoLower( &(_THIS->constraints),BT_FALSE );
2854  break;
2855  }
2856  }
2857  }
2858 
2859  /* 2) Check if upper constraints' bounds are present. */
2860  Constraints_setNoUpper( &(_THIS->constraints),BT_TRUE );
2861  if ( ubA_new != 0 )
2862  {
2863  for( i=0; i<nC; ++i )
2864  {
2865  if ( ubA_new[i] < QPOASES_INFTY )
2866  {
2867  Constraints_setNoUpper( &(_THIS->constraints),BT_FALSE );
2868  break;
2869  }
2870  }
2871  }
2872 
2873  /* 3) Determine implicit equality constraints and unbounded constraints. */
2874  if ( ( lbA_new != 0 ) && ( ubA_new != 0 ) )
2875  {
2876  for( i=0; i<nC; ++i )
2877  {
2878  if (Constraints_getType(&(_THIS->constraints),i) == ST_DISABLED)
2879  continue;
2880 
2881  if ( ( lbA_new[i] < -QPOASES_INFTY+_THIS->options.boundTolerance ) && ( ubA_new[i] > QPOASES_INFTY-_THIS->options.boundTolerance )
2882  && (_THIS->options.enableFarBounds == BT_FALSE))
2883  {
2884  Constraints_setType( &(_THIS->constraints),i,ST_UNBOUNDED );
2885  }
2886  else
2887  {
2888  if ( _THIS->options.enableEqualities && _THIS->lbA[i] > _THIS->ubA[i] - _THIS->options.boundTolerance
2889  && lbA_new[i] > ubA_new[i] - _THIS->options.boundTolerance)
2890  Constraints_setType( &(_THIS->constraints),i,ST_EQUALITY );
2891  else
2892  Constraints_setType( &(_THIS->constraints),i,ST_BOUNDED );
2893  }
2894  }
2895  }
2896  else
2897  {
2898  if ( ( lbA_new == 0 ) && ( ubA_new == 0 ) )
2899  {
2900  for( i=0; i<nC; ++i )
2901  Constraints_setType( &(_THIS->constraints),i,ST_UNBOUNDED );
2902  }
2903  else
2904  {
2905  for( i=0; i<nC; ++i )
2906  Constraints_setType( &(_THIS->constraints),i,ST_BOUNDED );
2907  }
2908  }
2909 
2910  return SUCCESSFUL_RETURN;
2911 }
2912 
2913 
2914 /*
2915  * c h o l e s k y D e c o m p o s i t i o n P r o j e c t e d
2916  */
2917 returnValue QProblem_computeProjectedCholesky( QProblem* _THIS )
2918 {
2919  int i, j;
2920  int nV = QProblem_getNV( _THIS );
2921  int nZ = QProblem_getNZ( _THIS );
2922  int nFR = QProblem_getNFR( _THIS );
2923 
2924  int *FR_idx, *AC_idx;
2925 
2926  long info = 0;
2927  unsigned long _nZ = (unsigned long)nZ, _nV = NVMAX;
2928 
2929  /* Revert to unprotected Cholesky decomposition */
2930  if ( QProblem_getNFX( _THIS ) + QProblem_getNAC( _THIS ) == 0 )
2931  return QProblemBCPY_computeCholesky( _THIS );
2932 
2933  /* 1) Initialises R with all zeros. */
2934  for( i=0; i<NVMAX*NVMAX; ++i )
2935  _THIS->R[i] = 0.0;
2936 
2937  /* Do not do anything for empty null spaces (important for LP case, HST_ZERO !)*/
2938  if ( nZ == 0 ) /* nZ == nV - QProblem_getNFX( _THIS ) - QProblem_getNAC( _THIS ) */
2939  return SUCCESSFUL_RETURN;
2940 
2941  /* 2) Calculate Cholesky decomposition of projected Hessian Z'*H*Z. */
2942  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
2943  Indexlist_getNumberArray( Constraints_getActive( &(_THIS->constraints) ),&AC_idx );
2944 
2945  /* calculate Z'*H*Z */
2946  switch ( _THIS->hessianType )
2947  {
2948  case HST_ZERO:
2949  if ( QProblem_usingRegularisation( _THIS ) == BT_TRUE )
2950  {
2951  /*Id = createDiagSparseMat( nV, _THIS->regVal );
2952  DenseMatrix_bilinear( Id,Bounds_getFree( &(_THIS->bounds) ), nZ, _THIS->Q, NVMAX, _THIS->R, NVMAX );*/
2953  /*fprintf( stderr,"\n\n!!!!!!! NOT YET IMPLEMENTED !!!!!!!!!!!!\n\n" );*/
2954  }
2955  else
2956  {
2957  return THROWERROR( RET_CHOLESKY_OF_ZERO_HESSIAN );
2958  }
2959  break;
2960 
2961  case HST_IDENTITY:
2962  /*Id = createDiagSparseMat( nV, 1.0 );
2963  DenseMatrix_bilinear( Id,Bounds_getFree( &(_THIS->bounds) ), nZ, _THIS->Q, NVMAX, _THIS->R, NVMAX );*/
2964  /*fprintf( stderr,"\n\n!!!!!!! NOT YET IMPLEMENTED !!!!!!!!!!!!\n\n" );*/
2965  break;
2966 
2967  default:
2968  if ( Indexlist_getLength( Constraints_getActive( &(_THIS->constraints)) ) == 0 ) {
2969  /* make Z trivial */
2970  for ( j=0; j < nZ; ++j ) {
2971  for ( i=0; i < nV; ++i )
2972  QQ(i,j) = 0.0;
2973  QQ(FR_idx[j],j) = 1.0;
2974  }
2975  /* now Z is trivial, and so is Z'HZ */
2976  for ( j=0; j < nFR; ++j )
2977  DenseMatrix_getCol( _THIS->H, FR_idx[j], Bounds_getFree( &(_THIS->bounds) ), 1.0, &(_THIS->R[j*NVMAX]));
2978  } else {
2979  /* _THIS is expensive if Z is large! */
2980  DenseMatrix_bilinear( _THIS->H, Bounds_getFree( &(_THIS->bounds) ), nZ, _THIS->Q, NVMAX, _THIS->R, NVMAX);
2981  }
2982  }
2983 
2984  /* R'*R = Z'*H*Z */
2985  POTRF( "U", &_nZ, _THIS->R, &_nV, &info );
2986 
2987  /* <0 = invalid call, =0 ok, >0 not spd */
2988  if (info > 0) {
2989  if ( _THIS->R[0] < 0.0 )
2990  {
2991  /* Cholesky decomposition has tunneled a negative
2992  * diagonal element. */
2993  _THIS->options.epsRegularisation = qpOASES_getMin( -_THIS->R[0]+_THIS->options.epsRegularisation,qpOASES_getSqrt(qpOASES_getAbs(_THIS->options.epsRegularisation)) );
2994  }
2995 
2996  _THIS->hessianType = HST_SEMIDEF;
2997  return RET_HESSIAN_NOT_SPD;
2998  }
2999 
3000  /* zero first subdiagonal to make givens updates work */
3001  for (i=0;i<nZ-1;++i)
3002  RR(i+1,i) = 0.0;
3003 
3004  return SUCCESSFUL_RETURN;
3005 }
3006 
3007 
3008 /*
3009  * s e t u p T Q f a c t o r i s a t i o n
3010  */
3011 returnValue QProblem_setupTQfactorisation( QProblem* _THIS )
3012 {
3013  int i, ii;
3014  int nFR = QProblem_getNFR( _THIS );
3015 
3016  int* FR_idx;
3017  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
3018 
3019  /* 1) Set Q to unity matrix. */
3020  for( i=0; i<NVMAX*NVMAX; ++i )
3021  _THIS->Q[i] = 0.0;
3022 
3023  for( i=0; i<nFR; ++i )
3024  {
3025  ii = FR_idx[i];
3026  QQ(ii,i) = 1.0;
3027  }
3028 
3029  /* 2) Set T to zero matrix. */
3030  for( i=0; i<NVCMIN*NVCMIN; ++i )
3031  _THIS->T[i] = 0.0;
3032 
3033  return SUCCESSFUL_RETURN;
3034 }
3035 
3036 
3037 /*
3038  * o b t a i n A u x i l i a r y W o r k i n g S e t
3039  */
3040 returnValue QProblem_obtainAuxiliaryWorkingSet( QProblem* _THIS,
3041  const real_t* const xOpt, const real_t* const yOpt,
3042  Bounds* const guessedBounds, Constraints* const guessedConstraints,
3043  Bounds* auxiliaryBounds,Constraints* auxiliaryConstraints
3044  )
3045 {
3046  int i = 0;
3047  int nV = QProblem_getNV( _THIS );
3048  int nC = QProblem_getNC( _THIS );
3049 
3050  SubjectToStatus guessedStatus;
3051 
3052 
3053  /* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
3054  if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
3055  return THROWERROR( RET_INVALID_ARGUMENTS );
3056 
3057  if ( ( auxiliaryConstraints == 0 ) || ( auxiliaryConstraints == guessedConstraints ) )
3058  return THROWERROR( RET_INVALID_ARGUMENTS );
3059 
3060  /* 2) Setup working set of bounds for auxiliary initial QP. */
3061  if ( QProblemBCPY_obtainAuxiliaryWorkingSet( _THIS,xOpt,yOpt,guessedBounds, auxiliaryBounds ) != SUCCESSFUL_RETURN )
3062  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3063 
3064  /* 3) Setup working set of constraints for auxiliary initial QP. */
3065  if ( guessedConstraints != 0 )
3066  {
3067  /* If an initial working set is specific, use it!
3068  * Moreover, add all equality constraints if specified. */
3069  for( i=0; i<nC; ++i )
3070  {
3071  /* Add constraint only if it is not (goint to be) disabled! */
3072  guessedStatus = Constraints_getStatus( guessedConstraints,i );
3073 
3074  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
3075  if ( Constraints_getType( &(_THIS->constraints),i ) == ST_EQUALITY )
3076  {
3077  if ( Constraints_setupConstraint( auxiliaryConstraints,i,ST_LOWER ) != SUCCESSFUL_RETURN )
3078  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3079  }
3080  else
3081  #endif
3082  {
3083  if ( Constraints_setupConstraint( auxiliaryConstraints,i,guessedStatus ) != SUCCESSFUL_RETURN )
3084  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3085  }
3086  }
3087  }
3088  else /* No initial working set specified. */
3089  {
3090  /* Obtain initial working set by "clipping". */
3091  if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
3092  {
3093  for( i=0; i<nC; ++i )
3094  {
3095  if ( _THIS->Ax[i] - _THIS->lbA[i] <= _THIS->options.boundTolerance )
3096  {
3097  if ( Constraints_setupConstraint( auxiliaryConstraints,i,ST_LOWER ) != SUCCESSFUL_RETURN )
3098  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3099  continue;
3100  }
3101 
3102  if ( _THIS->ubA[i] - _THIS->Ax_u[i] <= _THIS->options.boundTolerance )
3103  {
3104  if ( Constraints_setupConstraint( auxiliaryConstraints,i,ST_UPPER ) != SUCCESSFUL_RETURN )
3105  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3106  continue;
3107  }
3108 
3109  /* Moreover, add all equality constraints if specified. */
3110  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
3111  if ( Constraints_getType( &(_THIS->constraints),i ) == ST_EQUALITY )
3112  {
3113  if ( Constraints_setupConstraint( auxiliaryConstraints,i,ST_LOWER ) != SUCCESSFUL_RETURN )
3114  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3115  }
3116  else
3117  #endif
3118  {
3119  if ( Constraints_setupConstraint( auxiliaryConstraints,i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
3120  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3121  }
3122  }
3123  }
3124 
3125  /* Obtain initial working set in accordance to sign of dual solution vector. */
3126  if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
3127  {
3128  for( i=0; i<nC; ++i )
3129  {
3130  if ( yOpt[nV+i] > QPOASES_EPS )
3131  {
3132  if ( Constraints_setupConstraint( auxiliaryConstraints,i,ST_LOWER ) != SUCCESSFUL_RETURN )
3133  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3134  continue;
3135  }
3136 
3137  if ( yOpt[nV+i] < -QPOASES_EPS )
3138  {
3139  if ( Constraints_setupConstraint( auxiliaryConstraints,i,ST_UPPER ) != SUCCESSFUL_RETURN )
3140  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3141  continue;
3142  }
3143 
3144  /* Moreover, add all equality constraints if specified. */
3145  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
3146  if ( Constraints_getType( &(_THIS->constraints),i ) == ST_EQUALITY )
3147  {
3148  if ( Constraints_setupConstraint( auxiliaryConstraints,i,ST_LOWER ) != SUCCESSFUL_RETURN )
3149  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3150  }
3151  else
3152  #endif
3153  {
3154  if ( Constraints_setupConstraint( auxiliaryConstraints,i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
3155  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3156  }
3157  }
3158  }
3159 
3160  /* If xOpt and yOpt are null pointer and no initial working is specified,
3161  * start with empty working set (or implicitly fixed bounds and equality constraints only)
3162  * for auxiliary QP. */
3163  if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
3164  {
3165  for( i=0; i<nC; ++i )
3166  {
3167  /* Only add all equality constraints if specified. */
3168  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
3169  if ( Constraints_getType( &(_THIS->constraints),i ) == ST_EQUALITY )
3170  {
3171  if ( Constraints_setupConstraint( auxiliaryConstraints,i,ST_LOWER ) != SUCCESSFUL_RETURN )
3172  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3173  }
3174  else
3175  #endif
3176  {
3177  if ( Constraints_setupConstraint( auxiliaryConstraints,i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
3178  return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
3179  }
3180  }
3181  }
3182  }
3183 
3184  return SUCCESSFUL_RETURN;
3185 }
3186 
3187 
3188 
3189 /*
3190  * s e t u p A u x i l i a r y W o r k i n g S e t
3191  */
3192 returnValue QProblem_setupAuxiliaryWorkingSet( QProblem* _THIS,
3193  Bounds* const auxiliaryBounds,
3194  Constraints* const auxiliaryConstraints,
3195  BooleanType setupAfresh
3196  )
3197 {
3198  int i;
3199  int nV = QProblem_getNV( _THIS );
3200  int nC = QProblem_getNC( _THIS );
3201  BooleanType WSisTrivial = BT_TRUE;
3202 
3203  BooleanType updateCholesky;
3204  BooleanType was_fulli = _THIS->options.enableFullLITests;
3205  real_t backupEpsLITests = _THIS->options.epsLITests;
3206 
3207  /* consistency checks */
3208  if ( auxiliaryBounds != 0 )
3209  {
3210  for( i=0; i<nV; ++i )
3211  if ( ( Bounds_getStatus( &(_THIS->bounds),i ) == ST_UNDEFINED ) || ( Bounds_getStatus( auxiliaryBounds,i ) == ST_UNDEFINED ) )
3212  return THROWERROR( RET_UNKNOWN_BUG );
3213  }
3214  else
3215  {
3216  return THROWERROR( RET_INVALID_ARGUMENTS );
3217  }
3218 
3219  if ( auxiliaryConstraints != 0 )
3220  {
3221  for( i=0; i<nC; ++i )
3222  if ( ( Constraints_getStatus( &(_THIS->constraints),i ) == ST_UNDEFINED ) || ( Constraints_getStatus( auxiliaryConstraints,i ) == ST_UNDEFINED ) )
3223  return THROWERROR( RET_UNKNOWN_BUG );
3224  }
3225  else
3226  {
3227  return THROWERROR( RET_INVALID_ARGUMENTS );
3228  }
3229 
3230  /* Check for trivial working set (all and only bounds active) */
3231  for (i = 0; i < nV; i++)
3232  if (Bounds_getStatus( auxiliaryBounds,i) == ST_INACTIVE)
3233  {
3234  WSisTrivial = BT_FALSE;
3235  break;
3236  }
3237  for (i = 0; i < nC; i++)
3238 /* (ckirches) here we chose to ignore an invalid ST_INACTIVE on
3239  constraints that are ST_EQUALITies or may just have become equalities*/
3240  if ( (Constraints_getType( &(_THIS->constraints),i) == ST_EQUALITY) /* NOT auxiliaryConstraints here*/
3241  || (Constraints_getStatus(auxiliaryConstraints,i) != ST_INACTIVE) )
3242  {
3243  WSisTrivial = BT_FALSE;
3244  break;
3245  }
3246 
3247  if (WSisTrivial == BT_TRUE)
3248  {
3249  for (i = 0; i < nV; i++)
3250  if (Bounds_getStatus( &(_THIS->bounds),i) == ST_INACTIVE)
3251  Bounds_moveFreeToFixed( &(_THIS->bounds),i, Bounds_getStatus( auxiliaryBounds,i));
3252 
3253  return SUCCESSFUL_RETURN;
3254  }
3255 
3256 
3257  /* I) SETUP CHOLESKY FLAG:
3258  * Cholesky decomposition shall only be updated if working set
3259  * shall be updated (i.e. NOT setup afresh!) */
3260  if ( setupAfresh == BT_TRUE )
3261  updateCholesky = BT_FALSE;
3262  else
3263  updateCholesky = BT_TRUE;
3264 
3265  _THIS->options.enableFullLITests = BT_FALSE;
3266  /* _THIS->options.epsLITests = 1e-1; */
3267 
3268  /* II) REMOVE FORMERLY ACTIVE (CONSTRAINTS') BOUNDS (IF NECESSARY): */
3269  if ( setupAfresh == BT_FALSE )
3270  {
3271  /* 1) Remove all active constraints that shall be inactive or disabled AND
3272  * all active constraints that are active at the wrong bound. */
3273  for( i=0; i<nC; ++i )
3274  {
3275  if ( ( Constraints_getStatus( &(_THIS->constraints),i ) == ST_LOWER ) && ( Constraints_getStatus( auxiliaryConstraints,i ) != ST_LOWER ) )
3276  if ( QProblem_removeConstraint( _THIS,i,updateCholesky,BT_FALSE,_THIS->options.enableNZCTests ) != SUCCESSFUL_RETURN )
3277  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
3278 
3279  if ( ( Constraints_getStatus( &(_THIS->constraints),i ) == ST_UPPER ) && ( Constraints_getStatus( auxiliaryConstraints,i ) != ST_UPPER ) )
3280  if ( QProblem_removeConstraint( _THIS,i,updateCholesky,BT_FALSE,_THIS->options.enableNZCTests ) != SUCCESSFUL_RETURN )
3281  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
3282  }
3283 
3284  /* 2) Remove all active bounds that shall be inactive AND
3285  * all active bounds that are active at the wrong bound. */
3286  for( i=0; i<nV; ++i )
3287  {
3288  if ( ( Bounds_getStatus( &(_THIS->bounds),i ) == ST_LOWER ) && ( Bounds_getStatus( auxiliaryBounds,i ) != ST_LOWER ) )
3289  if ( QProblem_removeBound( _THIS,i,updateCholesky,BT_FALSE,_THIS->options.enableNZCTests ) != SUCCESSFUL_RETURN )
3290  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
3291 
3292  if ( ( Bounds_getStatus( &(_THIS->bounds),i ) == ST_UPPER ) && ( Bounds_getStatus( auxiliaryBounds,i ) != ST_UPPER ) )
3293  if ( QProblem_removeBound( _THIS,i,updateCholesky,BT_FALSE,_THIS->options.enableNZCTests ) != SUCCESSFUL_RETURN )
3294  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
3295  }
3296  }
3297 
3298 
3299  /* III) ADD NEWLY ACTIVE (CONSTRAINTS') BOUNDS: */
3300 
3301  /* 1) Add all equality bounds. */
3302  for( i=0; i<nV; ++i )
3303  {
3304  /*if ( ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY ) && ( ( Bounds_getStatus( &(_THIS->bounds),i ) == ST_INACTIVE ) && ( Bounds_getStatus( auxiliaryBounds,i ) != ST_INACTIVE ) ) )
3305 
3306  (ckirches) force equalities active*/
3307 
3308  if ( ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY ) && ( Bounds_getStatus( &(_THIS->bounds),i ) == ST_INACTIVE ) )
3309  {
3310  /* assert ( Bounds_getStatus( auxiliaryBounds,i ) != ST_INACTIVE ); */
3311  /* No check for linear independence necessary. */
3312  if ( QProblem_addBound( _THIS,i,ST_LOWER,updateCholesky,BT_TRUE ) != SUCCESSFUL_RETURN ) /* was Bounds_getStatus( auxiliaryBounds,i ) */
3313  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
3314  }
3315  }
3316 
3317  /* 2) Add all equality constraints. */
3318  for( i=0; i<nC; ++i )
3319  {
3320  /*if ( ( Constraints_getType( &(_THIS->constraints),i ) == ST_EQUALITY ) && ( ( Constraints_getStatus( &(_THIS->constraints),i ) == ST_INACTIVE ) && ( Constraints_getStatus( auxiliaryConstraints,i ) != ST_INACTIVE ) ) )
3321 
3322  (ckirches) force equalities active */
3323 
3324  if ( ( Constraints_getType( &(_THIS->constraints),i ) == ST_EQUALITY ) && ( Constraints_getStatus( &(_THIS->constraints),i ) == ST_INACTIVE ) )
3325  {
3326  /* assert ( Constraints_getStatus( auxiliaryConstraints,i ) != ST_INACTIVE ); */
3327  /* Add constraint only if it is linearly independent from the current working set. */
3328  if ( QProblem_addConstraint_checkLI( _THIS,i ) == RET_LINEARLY_INDEPENDENT )
3329  {
3330  if ( QProblem_addConstraint( _THIS,i,ST_LOWER,updateCholesky,BT_TRUE ) != SUCCESSFUL_RETURN ) /* was Constraints_getStatus( auxiliaryConstraints,i )*/
3331  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
3332  }
3333  else
3334  {
3335  /* Equalities are not linearly independent! */
3336  Constraints_setType( &(_THIS->constraints),i, ST_BOUNDED );
3337  }
3338  }
3339  }
3340 
3341 
3342  /* 3) Add all inactive bounds that shall be active AND
3343  * all formerly active bounds that have been active at the wrong bound. */
3344  for( i=0; i<nV; ++i )
3345  {
3346  if ( ( Bounds_getType( &(_THIS->bounds),i ) != ST_EQUALITY ) && ( ( Bounds_getStatus( &(_THIS->bounds),i ) == ST_INACTIVE ) && ( Bounds_getStatus( auxiliaryBounds,i ) != ST_INACTIVE ) ) )
3347  {
3348  /* Add bound only if it is linearly independent from the current working set. */
3349  if ( QProblem_addBound_checkLI( _THIS,i ) == RET_LINEARLY_INDEPENDENT )
3350  {
3351  if ( QProblem_addBound( _THIS,i,Bounds_getStatus( auxiliaryBounds,i ),updateCholesky,BT_TRUE ) != SUCCESSFUL_RETURN )
3352  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
3353  }
3354  }
3355  }
3356 
3357  /* 4) Add all inactive constraints that shall be active AND
3358  * all formerly active constraints that have been active at the wrong bound. */
3359  for( i=0; i<nC; ++i )
3360  {
3361  if ( ( Constraints_getType( &(_THIS->constraints),i ) != ST_EQUALITY ) && ( Constraints_getStatus( auxiliaryConstraints,i ) != ST_INACTIVE ) )
3362  {
3363  /* formerly inactive */
3364  if ( Constraints_getStatus( &(_THIS->constraints),i ) == ST_INACTIVE )
3365  {
3366  /* Add constraint only if it is linearly independent from the current working set. */
3367  if ( QProblem_addConstraint_checkLI( _THIS,i ) == RET_LINEARLY_INDEPENDENT )
3368  {
3369  if ( QProblem_addConstraint( _THIS,i,Constraints_getStatus( auxiliaryConstraints,i ),updateCholesky,BT_TRUE ) != SUCCESSFUL_RETURN )
3370  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
3371  }
3372  }
3373  }
3374  }
3375 
3376  _THIS->options.enableFullLITests = was_fulli;
3377  _THIS->options.epsLITests = backupEpsLITests;
3378 
3379  return SUCCESSFUL_RETURN;
3380 }
3381 
3382 
3383 /*
3384  * s e t u p A u x i l i a r y Q P s o l u t i o n
3385  */
3386 returnValue QProblem_setupAuxiliaryQPsolution( QProblem* _THIS,
3387  const real_t* const xOpt, const real_t* const yOpt
3388  )
3389 {
3390  int i, j;
3391  int nV = QProblem_getNV( _THIS );
3392  int nC = QProblem_getNC( _THIS );
3393 
3394 
3395  /* Setup primal/dual solution vector for auxiliary initial QP:
3396  * if a null pointer is passed, a zero vector is assigned;
3397  * old solution vector is kept if pointer to internal solution vevtor is passed. */
3398  if ( xOpt != 0 )
3399  {
3400  if ( xOpt != _THIS->x )
3401  for( i=0; i<nV; ++i )
3402  _THIS->x[i] = xOpt[i];
3403 
3404  DenseMatrix_times(_THIS->A,1, 1.0, _THIS->x, nV, 0.0, _THIS->Ax, nC);
3405 
3406  for ( j=0; j<nC; ++j )
3407  {
3408  _THIS->Ax_l[j] = _THIS->Ax[j];
3409  _THIS->Ax_u[j] = _THIS->Ax[j];
3410  }
3411  }
3412  else
3413  {
3414  for( i=0; i<nV; ++i )
3415  _THIS->x[i] = 0.0;
3416 
3417  for ( j=0; j<nC; ++j )
3418  {
3419  _THIS->Ax[j] = 0.0;
3420  _THIS->Ax_l[j] = 0.0;
3421  _THIS->Ax_u[j] = 0.0;
3422  }
3423  }
3424 
3425  if ( yOpt != 0 )
3426  {
3427  if ( yOpt != _THIS->y )
3428  for( i=0; i<nV+nC; ++i )
3429  _THIS->y[i] = yOpt[i];
3430  }
3431  else
3432  {
3433  for( i=0; i<nV+nC; ++i )
3434  _THIS->y[i] = 0.0;
3435  }
3436 
3437  return SUCCESSFUL_RETURN;
3438 }
3439 
3440 
3441 /*
3442  * s e t u p A u x i l i a r y Q P g r a d i e n t
3443  */
3444 returnValue QProblem_setupAuxiliaryQPgradient( QProblem* _THIS )
3445 {
3446  int i;
3447  int nV = QProblem_getNV( _THIS );
3448  int nC = QProblem_getNC( _THIS );
3449 
3450 
3451  /* Setup gradient vector: g = -H*x + [Id A]'*[yB yC]
3452  * = yB - H*x + A'*yC. */
3453  switch ( _THIS->hessianType )
3454  {
3455  case HST_ZERO:
3456  if ( QProblem_usingRegularisation( _THIS ) == BT_FALSE )
3457  for ( i=0; i<nV; ++i )
3458  _THIS->g[i] = _THIS->y[i];
3459  else
3460  for ( i=0; i<nV; ++i )
3461  _THIS->g[i] = _THIS->y[i] - _THIS->regVal*_THIS->x[i];
3462  break;
3463 
3464  case HST_IDENTITY:
3465  for ( i=0; i<nV; ++i )
3466  _THIS->g[i] = _THIS->y[i] - _THIS->x[i];
3467  break;
3468 
3469  default:
3470  /* y'*Id */
3471  for ( i=0; i<nV; ++i )
3472  _THIS->g[i] = _THIS->y[i];
3473 
3474  /* - H*x */
3475  DenseMatrix_times(_THIS->H,1, -1.0, _THIS->x, nV, 1.0, _THIS->g, nV);
3476  break;
3477  }
3478 
3479  /* + A'*yC */
3480  DenseMatrix_transTimes(_THIS->A,1, 1.0, &(_THIS->y[nV]), nC, 1.0, _THIS->g, nV);
3481 
3482  return SUCCESSFUL_RETURN;
3483 }
3484 
3485 
3486 /*
3487  * s e t u p A u x i l i a r y Q P b o u n d s
3488  */
3489 returnValue QProblem_setupAuxiliaryQPbounds( QProblem* _THIS,
3490  Bounds* const auxiliaryBounds,
3491  Constraints* const auxiliaryConstraints,
3492  BooleanType useRelaxation
3493  )
3494 {
3495  int i;
3496  int nV = QProblem_getNV( _THIS );
3497  int nC = QProblem_getNC( _THIS );
3498 
3499 
3500  /* 1) Setup bound vectors. */
3501  for ( i=0; i<nV; ++i )
3502  {
3503  switch ( Bounds_getStatus( &(_THIS->bounds),i ) )
3504  {
3505  case ST_INACTIVE:
3506  if ( useRelaxation == BT_TRUE )
3507  {
3508  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
3509  {
3510  _THIS->lb[i] = _THIS->x[i];
3511  _THIS->ub[i] = _THIS->x[i];
3512  }
3513  else
3514  {
3515  /* If a bound is inactive although it was supposed to be
3516  * active by the auxiliaryBounds it could not be added
3517  * due to linear dependence. Thus set it "strongly inactive". */
3518  if ( Bounds_getStatus( auxiliaryBounds,i ) == ST_LOWER )
3519  _THIS->lb[i] = _THIS->x[i];
3520  else
3521  _THIS->lb[i] = _THIS->x[i] - _THIS->options.boundRelaxation;
3522 
3523  if ( Bounds_getStatus( auxiliaryBounds,i ) == ST_UPPER )
3524  _THIS->ub[i] = _THIS->x[i];
3525  else
3526  _THIS->ub[i] = _THIS->x[i] + _THIS->options.boundRelaxation;
3527  }
3528  }
3529  break;
3530 
3531  case ST_LOWER:
3532  _THIS->lb[i] = _THIS->x[i];
3533  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
3534  {
3535  _THIS->ub[i] = _THIS->x[i];
3536  }
3537  else
3538  {
3539  if ( useRelaxation == BT_TRUE )
3540  _THIS->ub[i] = _THIS->x[i] + _THIS->options.boundRelaxation;
3541  }
3542  break;
3543 
3544  case ST_UPPER:
3545  _THIS->ub[i] = _THIS->x[i];
3546  if ( Bounds_getType( &(_THIS->bounds),i ) == ST_EQUALITY )
3547  {
3548  _THIS->lb[i] = _THIS->x[i];
3549  }
3550  else
3551  {
3552  if ( useRelaxation == BT_TRUE )
3553  _THIS->lb[i] = _THIS->x[i] - _THIS->options.boundRelaxation;
3554  }
3555  break;
3556 
3557  case ST_DISABLED:
3558  break;
3559 
3560  default:
3561  return THROWERROR( RET_UNKNOWN_BUG );
3562  }
3563  }
3564 
3565  /* 2) Setup constraints vectors. */
3566  for ( i=0; i<nC; ++i )
3567  {
3568  switch ( Constraints_getStatus( &(_THIS->constraints),i ) )
3569  {
3570  case ST_INACTIVE:
3571  if ( useRelaxation == BT_TRUE )
3572  {
3573  if ( Constraints_getType( &(_THIS->constraints),i ) == ST_EQUALITY )
3574  {
3575  _THIS->lbA[i] = _THIS->Ax_l[i];
3576  _THIS->ubA[i] = _THIS->Ax_u[i];
3577  }
3578  else
3579  {
3580  /* If a constraint is inactive although it was supposed to be
3581  * active by the auxiliaryConstraints, it could not be added
3582  * due to linear dependence. Thus set it "strongly inactive". */
3583  if ( Constraints_getStatus( auxiliaryConstraints,i ) == ST_LOWER )
3584  _THIS->lbA[i] = _THIS->Ax_l[i];
3585  else
3586  _THIS->lbA[i] = _THIS->Ax_l[i] - _THIS->options.boundRelaxation;
3587 
3588  if ( Constraints_getStatus( auxiliaryConstraints,i ) == ST_UPPER )
3589  _THIS->ubA[i] = _THIS->Ax_u[i];
3590  else
3591  _THIS->ubA[i] = _THIS->Ax_u[i] + _THIS->options.boundRelaxation;
3592  }
3593  }
3594  break;
3595 
3596  case ST_LOWER:
3597  _THIS->lbA[i] = _THIS->Ax_l[i];
3598  if ( Constraints_getType( &(_THIS->constraints),i ) == ST_EQUALITY )
3599  {
3600  _THIS->ubA[i] = _THIS->Ax_l[i];
3601  }
3602  else
3603  {
3604  if ( useRelaxation == BT_TRUE )
3605  _THIS->ubA[i] = _THIS->Ax_l[i] + _THIS->options.boundRelaxation;
3606  }
3607  break;
3608 
3609  case ST_UPPER:
3610  _THIS->ubA[i] = _THIS->Ax_u[i];
3611  if ( Constraints_getType( &(_THIS->constraints),i ) == ST_EQUALITY )
3612  {
3613  _THIS->lbA[i] = _THIS->Ax_u[i];
3614  }
3615  else
3616  {
3617  if ( useRelaxation == BT_TRUE )
3618  _THIS->lbA[i] = _THIS->Ax_u[i] - _THIS->options.boundRelaxation;
3619  }
3620  break;
3621 
3622  case ST_DISABLED:
3623  break;
3624 
3625  default:
3626  return THROWERROR( RET_UNKNOWN_BUG );
3627  }
3628  _THIS->Ax_l[i] = _THIS->Ax_l[i] - _THIS->lbA[i];
3629  _THIS->Ax_u[i] = _THIS->ubA[i] - _THIS->Ax_u[i];
3630  }
3631 
3632  return SUCCESSFUL_RETURN;
3633 }
3634 
3635 
3636 /*
3637  * a d d C o n s t r a i n t
3638  */
3639 returnValue QProblem_addConstraint( QProblem* _THIS,
3640  int number, SubjectToStatus C_status,
3641  BooleanType updateCholesky,
3642  BooleanType ensureLI
3643  )
3644 {
3645  int i, j, ii;
3646 
3647  returnValue ensureLIreturnvalue;
3648 
3649  int nFR, nAC, nZ, tcol;
3650  int* FR_idx;
3651 
3652  myStatic real_t aFR[NVMAX];
3653  myStatic real_t wZ[NVMAX];
3654 
3655  real_t c, s, nu;
3656 
3657 
3658  /* consistency checks */
3659  if ( Constraints_getStatus( &(_THIS->constraints),number ) != ST_INACTIVE )
3660  return THROWERROR( RET_CONSTRAINT_ALREADY_ACTIVE );
3661 
3662  if ( ( Constraints_getNC( &(_THIS->constraints) ) - QProblem_getNAC( _THIS ) ) == Constraints_getNUC( &(_THIS->constraints) ) )
3663  return THROWERROR( RET_ALL_CONSTRAINTS_ACTIVE );
3664 
3665  if ( ( QProblem_getStatus( _THIS ) == QPS_NOTINITIALISED ) ||
3666  ( QProblem_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
3667  ( QProblem_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
3668  ( QProblem_getStatus( _THIS ) == QPS_SOLVED ) )
3669  {
3670  return THROWERROR( RET_UNKNOWN_BUG );
3671  }
3672 
3673 
3674  /* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
3675  * i.e. remove a constraint or bound if linear dependence occurs. */
3676  /* check for LI only if Cholesky decomposition shall be updated! */
3677  if ( ( updateCholesky == BT_TRUE ) && ( ensureLI == BT_TRUE ) )
3678  {
3679  ensureLIreturnvalue = QProblem_addConstraint_ensureLI( _THIS,number,C_status );
3680 
3681  switch ( ensureLIreturnvalue )
3682  {
3683  case SUCCESSFUL_RETURN:
3684  break;
3685 
3686  case RET_LI_RESOLVED:
3687  break;
3688 
3689  case RET_ENSURELI_FAILED_NOINDEX:
3690  return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;
3691 
3692  case RET_ENSURELI_FAILED_CYCLING:
3693  return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;
3694 
3695  case RET_ENSURELI_DROPPED:
3696  return SUCCESSFUL_RETURN;
3697 
3698  default:
3699  return THROWERROR( RET_ENSURELI_FAILED );
3700  }
3701  }
3702 
3703  /* some definitions */
3704  nFR = QProblem_getNFR( _THIS );
3705  nAC = QProblem_getNAC( _THIS );
3706  nZ = QProblem_getNZ( _THIS );
3707 
3708  tcol = _THIS->sizeT - nAC;
3709 
3710  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
3711 
3712  for( i=0; i<nZ; ++i )
3713  wZ[i] = 0.0;
3714 
3715 
3716  /* II) ADD NEW ACTIVE CONSTRAINT TO MATRIX T: */
3717  /* 1) Add row [wZ wY] = aFR'*[Z Y] to the end of T: assign aFR. */
3718  DenseMatrix_getRow(_THIS->A,number, Bounds_getFree( &(_THIS->bounds) ), 1.0, aFR);
3719 
3720  /* calculate wZ */
3721  for( i=0; i<nFR; ++i )
3722  {
3723  ii = FR_idx[i];
3724  for( j=0; j<nZ; ++j )
3725  wZ[j] += aFR[i] * QQ(ii,j);
3726  }
3727 
3728  /* 2) Calculate wY and store it directly into T. */
3729  if ( nAC > 0 )
3730  {
3731  for( j=0; j<nAC; ++j )
3732  TT(nAC,tcol+j) = 0.0;
3733  for( i=0; i<nFR; ++i )
3734  {
3735  ii = FR_idx[i];
3736  for( j=0; j<nAC; ++j )
3737  TT(nAC,tcol+j) += aFR[i] * QQ(ii,nZ+j);
3738  }
3739  }
3740 
3741  if ( nZ > 0 )
3742  {
3743  /* II) RESTORE TRIANGULAR FORM OF T: */
3744  /* Use column-wise Givens rotations to restore reverse triangular form
3745  * of T, simultanenous change of Q (i.e. Z) and R. */
3746  for( j=0; j<nZ-1; ++j )
3747  {
3748  QProblemB_computeGivens( wZ[j+1],wZ[j], &wZ[j+1],&wZ[j],&c,&s );
3749  nu = s/(1.0+c);
3750 
3751  for( i=0; i<nFR; ++i )
3752  {
3753  ii = FR_idx[i];
3754  QProblemB_applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), &QQ(ii,1+j),&QQ(ii,j) );
3755  }
3756 
3757  if ( ( updateCholesky == BT_TRUE ) &&
3758  ( _THIS->hessianType != HST_ZERO ) && ( _THIS->hessianType != HST_IDENTITY ) )
3759  {
3760  for( i=0; i<=j+1; ++i )
3761  QProblemB_applyGivens( c,s,nu,RR(i,1+j),RR(i,j), &RR(i,1+j),&RR(i,j) );
3762  }
3763  }
3764 
3765  TT(nAC,tcol-1) = wZ[nZ-1];
3766 
3767 
3768  if ( ( updateCholesky == BT_TRUE ) &&
3769  ( _THIS->hessianType != HST_ZERO ) && ( _THIS->hessianType != HST_IDENTITY ) )
3770  {
3771  /* III) RESTORE TRIANGULAR FORM OF R:
3772  * Use row-wise Givens rotations to restore upper triangular form of R. */
3773  for( i=0; i<nZ-1; ++i )
3774  {
3775  QProblemB_computeGivens( RR(i,i),RR(1+i,i), &RR(i,i),&RR(1+i,i),&c,&s );
3776  nu = s/(1.0+c);
3777 
3778  for( j=(1+i); j<(nZ-1); ++j ) /* last column of R is thrown away */
3779  QProblemB_applyGivens( c,s,nu,RR(i,j),RR(1+i,j), &RR(i,j),&RR(1+i,j) );
3780  }
3781  /* last column of R is thrown away */
3782  for( i=0; i<nZ; ++i )
3783  RR(i,nZ-1) = 0.0;
3784  }
3785  }
3786 
3787 
3788  /* IV) UPDATE INDICES */
3789  _THIS->tabularOutput.idxAddC = number;
3790 
3791  if ( Constraints_moveInactiveToActive( &(_THIS->constraints),number,C_status ) != SUCCESSFUL_RETURN )
3792  return THROWERROR( RET_ADDCONSTRAINT_FAILED );
3793 
3794 
3795  return SUCCESSFUL_RETURN;
3796 }
3797 
3798 
3799 
3800 /*
3801  * a d d C o n s t r a i n t _ c h e c k L I
3802  */
3803 returnValue QProblem_addConstraint_checkLI( QProblem* _THIS, int number )
3804 {
3805  returnValue returnvalue = RET_LINEARLY_DEPENDENT;
3806 
3807  int i, j, ii;
3808  int nV = QProblem_getNV( _THIS );
3809  int nFR = QProblem_getNFR( _THIS );
3810  int nZ = QProblem_getNZ( _THIS );
3811  int nC = QProblem_getNC( _THIS );
3812  int nAC = QProblem_getNAC( _THIS );
3813  int nFX = QProblem_getNFX( _THIS );
3814  int dim;
3815 
3816  int *FX_idx, *AC_idx, *IAC_idx, *FR_idx;
3817 
3818  myStatic real_t delta_g[NVMAX];
3819  myStatic real_t delta_xFX[NVMAX];
3820  myStatic real_t delta_xFR[NVMAX];
3821  myStatic real_t delta_yAC[NCMAX];
3822  myStatic real_t delta_yFX[NVMAX];
3823 
3824  myStatic real_t nul[NVCMAX];
3825  myStatic real_t Arow[NVMAX];
3826 
3827  real_t weight = 0.0;
3828  real_t zero = 0.0;
3829 
3830  real_t sum, l2;
3831 
3832  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
3833 
3834 
3835  if (_THIS->options.enableFullLITests)
3836  {
3837  /*
3838  * expensive LI test. Backsolve with refinement using special right
3839  * hand side. This gives an estimate for what should be considered
3840  * "zero". We then check linear independence relative to _THIS estimate.
3841  */
3842  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
3843  Indexlist_getNumberArray( Constraints_getActive( &(_THIS->constraints) ),&AC_idx );
3844  Indexlist_getNumberArray( Constraints_getInactive( &(_THIS->constraints) ),&IAC_idx );
3845 
3846  dim = (nC>nV)?nC:nV;
3847  for (ii = 0; ii < dim; ++ii)
3848  nul[ii]=0.0;
3849 
3850  DenseMatrix_getRow(_THIS->A,number, 0, 1.0, delta_g);
3851 
3852  returnvalue = QProblem_determineStepDirection( _THIS,delta_g,
3853  nul, nul, nul, nul,
3854  BT_FALSE, BT_FALSE,
3855  delta_xFX, delta_xFR, delta_yAC, delta_yFX);
3856 
3857  /* compute the weight in inf-norm */
3858  for (ii = 0; ii < nAC; ++ii)
3859  {
3860  real_t a = qpOASES_getAbs (delta_yAC[ii]);
3861  if (weight < a) weight = a;
3862  }
3863  for (ii = 0; ii < nFX; ++ii)
3864  {
3865  real_t a = qpOASES_getAbs (delta_yFX[ii]);
3866  if (weight < a) weight = a;
3867  }
3868 
3869  /* look at the "zero" in a relative inf-norm */
3870  for (ii = 0; ii < nFX; ++ii)
3871  {
3872  real_t a = qpOASES_getAbs (delta_xFX[ii]);
3873  if (zero < a) zero = a;
3874  }
3875  for (ii = 0; ii < nFR; ++ii)
3876  {
3877  real_t a = qpOASES_getAbs (delta_xFR[ii]);
3878  if (zero < a) zero = a;
3879  }
3880 
3881  /* relative test against zero in inf-norm */
3882  if (zero > _THIS->options.epsLITests * weight)
3883  returnvalue = RET_LINEARLY_INDEPENDENT;
3884 
3885  }
3886  else
3887  {
3888  /*
3889  * cheap LI test for constraint. Check if constraint <number> is
3890  * linearly independent from the the active ones (<=> is element of null
3891  * space of Afr).
3892  */
3893 
3894  DenseMatrix_getRow(_THIS->A,number, Bounds_getFree( &(_THIS->bounds) ), 1.0, Arow);
3895 
3896  l2 = 0.0;
3897  for (i = 0; i < nFR; i++)
3898  l2 += Arow[i]*Arow[i];
3899 
3900  for( j=0; j<nZ; ++j )
3901  {
3902  sum = 0.0;
3903  for( i=0; i<nFR; ++i )
3904  {
3905  ii = FR_idx[i];
3906  sum += Arow[i] * QQ(ii,j);
3907  }
3908 
3909  if ( qpOASES_getAbs( sum ) > _THIS->options.epsLITests*l2 )
3910  {
3911  /*fprintf(stdFile, "LI test: |sum| = %9.2e, l2 = %9.2e, var = %d\n", qpOASES_getAbs(sum), l2, jj+1); */
3912  returnvalue = RET_LINEARLY_INDEPENDENT;
3913  break;
3914  }
3915  }
3916  }
3917 
3918  return THROWINFO( returnvalue );
3919 }
3920 
3921 
3922 /*
3923  * a d d C o n s t r a i n t _ e n s u r e L I
3924  */
3925 returnValue QProblem_addConstraint_ensureLI( QProblem* _THIS, int number, SubjectToStatus C_status )
3926 {
3927  int i, j, ii, jj;
3928  #ifndef __SUPPRESSANYOUTPUT__
3929  myStatic char messageString[QPOASES_MAX_STRING_LENGTH];
3930  #endif
3931 
3932  int nV = QProblem_getNV( _THIS );
3933  int nFR = QProblem_getNFR( _THIS );
3934  int nFX = QProblem_getNFX( _THIS );
3935  int nAC = QProblem_getNAC( _THIS );
3936  int nZ = QProblem_getNZ( _THIS );
3937 
3938  int *FR_idx, *FX_idx, *AC_idx;
3939 
3940  myStatic real_t xiC[NCMAX];
3941  myStatic real_t xiC_TMP[NCMAX];
3942  myStatic real_t xiB[NVMAX];
3943  myStatic real_t Arow[NVMAX];
3944  myStatic real_t num[NVMAX];
3945 
3946  returnValue returnvalue = SUCCESSFUL_RETURN;
3947 
3948  real_t y_min = _THIS->options.maxDualJump;
3949  int y_min_number = -1;
3950  int y_min_number_bound = -1;
3951  BooleanType y_min_isBound = BT_FALSE;
3952 
3953 
3954  /* I) Check if new constraint is linearly independent from the active ones. */
3955  returnValue returnvalueCheckLI = QProblem_addConstraint_checkLI( _THIS,number );
3956 
3957  if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
3958  return THROWERROR( RET_ENSURELI_FAILED );
3959 
3960  if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
3961  return SUCCESSFUL_RETURN;
3962 
3963 
3964  /* II) NEW CONSTRAINT IS LINEARLY DEPENDENT: */
3965  /* 1) Determine coefficients of linear combination,
3966  * cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
3967  * An Algorithm for the Solution of the Parametric Quadratic Programming
3968  * Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
3969  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
3970  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
3971 
3972  DenseMatrix_getRow(_THIS->A,number, Bounds_getFree( &(_THIS->bounds) ), C_status == ST_LOWER ? 1.0 : -1.0, Arow);
3973 
3974  /* 2) Calculate xiC */
3975  if ( nAC > 0 )
3976  {
3977  for( i=0; i<nAC; ++i )
3978  {
3979  xiC_TMP[i] = 0.0;
3980  for( j=0; j<nFR; ++j )
3981  {
3982  jj = FR_idx[j];
3983  xiC_TMP[i] += QQ(jj,nZ+i) * Arow[j];
3984  }
3985  }
3986 
3987  if ( QProblem_backsolveT( _THIS,xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
3988  {
3989  returnvalue = RET_ENSURELI_FAILED_TQ;
3990  goto farewell;
3991  }
3992  }
3993 
3994  /* 3) Calculate xiB. */
3995  Indexlist_getNumberArray( Constraints_getActive( &(_THIS->constraints) ),&AC_idx );
3996 
3997  DenseMatrix_getRow(_THIS->A,number, Bounds_getFixed( &(_THIS->bounds) ), C_status == ST_LOWER ? 1.0 : -1.0, xiB);
3998  DenseMatrix_subTransTimes(_THIS->A,Constraints_getActive( &(_THIS->constraints)), Bounds_getFixed( &(_THIS->bounds) ), 1, -1.0, xiC, nAC, 1.0, xiB, nFX);
3999 
4000  /* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
4001 
4002  /* 1) Constraints. */
4003  for( i=0; i<nAC; ++i )
4004  {
4005  ii = AC_idx[i];
4006  num[i] = _THIS->y[nV+ii];
4007  }
4008 
4009  QProblem_performRatioTestC( _THIS,nAC, AC_idx, &(_THIS->constraints), num, xiC, _THIS->options.epsNum, _THIS->options.epsDen, &y_min,&y_min_number);
4010 
4011  /* 2) Bounds. */
4012  for( i=0; i<nFX; ++i )
4013  {
4014  ii = FX_idx[i];
4015  num[i] = _THIS->y[ii];
4016  }
4017 
4018  QProblem_performRatioTestB( _THIS,nFX, FX_idx, &(_THIS->bounds),num, xiB, _THIS->options.epsNum, _THIS->options.epsDen, &y_min,&y_min_number_bound);
4019 
4020  if ( y_min_number_bound >= 0 )
4021  {
4022  y_min_number = y_min_number_bound;
4023  y_min_isBound = BT_TRUE;
4024  }
4025 
4026 
4027  /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
4028  if ( y_min_number >= 0 )
4029  {
4030  /* Update Lagrange multiplier... */
4031  for( i=0; i<nAC; ++i )
4032  {
4033  ii = AC_idx[i];
4034  _THIS->y[nV+ii] -= y_min * xiC[i];
4035  }
4036  for( i=0; i<nFX; ++i )
4037  {
4038  ii = FX_idx[i];
4039  _THIS->y[ii] -= y_min * xiB[i];
4040  }
4041 
4042  /* ... also for newly active constraint... */
4043  if ( C_status == ST_LOWER )
4044  _THIS->y[nV+number] = y_min;
4045  else
4046  _THIS->y[nV+number] = -y_min;
4047 
4048  /* ... and for constraint to be removed. */
4049  if ( y_min_isBound == BT_TRUE )
4050  {
4051  #ifndef __SUPPRESSANYOUTPUT__
4052  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"bound no. %d.",y_min_number );
4053  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_REMOVE_FROM_ACTIVESET,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
4054  #endif
4055 
4056  if ( QProblem_removeBound( _THIS,y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
4057  {
4058  returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
4059  goto farewell;
4060  }
4061  _THIS->tabularOutput.excRemB = 1;
4062 
4063  _THIS->y[y_min_number] = 0.0;
4064  }
4065  else
4066  {
4067  #ifndef __SUPPRESSANYOUTPUT__
4068  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"constraint no. %d.",y_min_number );
4069  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_REMOVE_FROM_ACTIVESET,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
4070  #endif
4071 
4072  if ( QProblem_removeConstraint( _THIS,y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
4073  {
4074  returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
4075  goto farewell;
4076  }
4077  _THIS->tabularOutput.excRemC = 1;
4078 
4079  _THIS->y[nV+y_min_number] = 0.0;
4080  }
4081  }
4082  else
4083  {
4084  if (_THIS->options.enableDropInfeasibles == BT_TRUE) {
4085  /* dropping of infeasible constraints according to drop priorities */
4086  returnvalue = QProblem_dropInfeasibles( _THIS,number, C_status, BT_FALSE, xiB, xiC);
4087  }
4088  else
4089  {
4090  /* no constraint/bound can be removed => QP is infeasible! */
4091  returnvalue = RET_ENSURELI_FAILED_NOINDEX;
4092  QProblem_setInfeasibilityFlag( _THIS,returnvalue,BT_FALSE );
4093  }
4094  }
4095 
4096 farewell:
4097  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_LI_RESOLVED,0,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
4098 
4099  return ( (returnvalue != SUCCESSFUL_RETURN) && (returnvalue != RET_ENSURELI_FAILED_NOINDEX ) ) ? THROWERROR (returnvalue) : returnvalue;
4100 }
4101 
4102 
4103 
4104 /*
4105  * a d d B o u n d
4106  */
4107 returnValue QProblem_addBound( QProblem* _THIS, int number, SubjectToStatus B_status,
4108  BooleanType updateCholesky,
4109  BooleanType ensureLI
4110  )
4111 {
4112  int i, j, ii;
4113  returnValue ensureLIreturnvalue;
4114 
4115  int nFR, nAC, nZ, tcol, lastfreenumber;
4116 
4117  int* FR_idx;
4118  myStatic real_t w[NVMAX];
4119  real_t c, s, nu;
4120  myStatic real_t tmp[NCMAX];
4121 
4122 
4123  /* consistency checks */
4124  if ( Bounds_getStatus( &(_THIS->bounds),number ) != ST_INACTIVE )
4125  return THROWERROR( RET_BOUND_ALREADY_ACTIVE );
4126 
4127  if ( QProblem_getNFR( _THIS ) == Bounds_getNUV( &(_THIS->bounds) ) )
4128  return THROWERROR( RET_ALL_BOUNDS_ACTIVE );
4129 
4130  if ( ( QProblem_getStatus( _THIS ) == QPS_NOTINITIALISED ) ||
4131  ( QProblem_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
4132  ( QProblem_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
4133  ( QProblem_getStatus( _THIS ) == QPS_SOLVED ) )
4134  {
4135  return THROWERROR( RET_UNKNOWN_BUG );
4136  }
4137 
4138 
4139  /* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
4140  * i.e. remove a constraint or bound if linear dependence occurs. */
4141  /* check for LI only if Cholesky decomposition shall be updated! */
4142  if ( updateCholesky == BT_TRUE && ensureLI == BT_TRUE )
4143  {
4144  ensureLIreturnvalue = QProblem_addBound_ensureLI( _THIS,number,B_status );
4145 
4146  switch ( ensureLIreturnvalue )
4147  {
4148  case SUCCESSFUL_RETURN:
4149  break;
4150 
4151  case RET_LI_RESOLVED:
4152  break;
4153 
4154  case RET_ENSURELI_FAILED_NOINDEX:
4155  return RET_ADDBOUND_FAILED_INFEASIBILITY;
4156 
4157  case RET_ENSURELI_FAILED_CYCLING:
4158  return RET_ADDBOUND_FAILED_INFEASIBILITY;
4159 
4160  case RET_ENSURELI_DROPPED:
4161  return SUCCESSFUL_RETURN;
4162 
4163  default:
4164  return THROWERROR( RET_ENSURELI_FAILED );
4165  }
4166  }
4167 
4168  /* some definitions */
4169  nFR = QProblem_getNFR( _THIS );
4170  nAC = QProblem_getNAC( _THIS );
4171  nZ = QProblem_getNZ( _THIS );
4172 
4173  tcol = _THIS->sizeT - nAC;
4174 
4175 
4176  /* II) SWAP INDEXLIST OF FREE VARIABLES:
4177  * move the variable to be fixed to the end of the list of free variables. */
4178  lastfreenumber = Indexlist_getLastNumber( Bounds_getFree( &(_THIS->bounds) ) );
4179  if ( lastfreenumber != number )
4180  if ( Bounds_swapFree( &(_THIS->bounds),number,lastfreenumber ) != SUCCESSFUL_RETURN )
4181  THROWERROR( RET_ADDBOUND_FAILED );
4182 
4183  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
4184 
4185 
4186  /* III) ADD NEW ACTIVE BOUND TO TOP OF MATRIX T: */
4187  /* 1) add row [wZ wY] = [Z Y](number) at the top of T: assign w */
4188  for( i=0; i<nFR; ++i )
4189  w[i] = QQ(FR_idx[nFR-1],i);
4190 
4191 
4192  /* 2) Use column-wise Givens rotations to restore reverse triangular form
4193  * of the first row of T, simultanenous change of Q (i.e. Z) and R. */
4194  for( j=0; j<nZ-1; ++j )
4195  {
4196  QProblemB_computeGivens( w[j+1],w[j], &w[j+1],&w[j],&c,&s );
4197  nu = s/(1.0+c);
4198 
4199  for( i=0; i<nFR; ++i )
4200  {
4201  ii = FR_idx[i];
4202  QProblemB_applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), &QQ(ii,1+j),&QQ(ii,j) );
4203  }
4204 
4205  if ( ( updateCholesky == BT_TRUE ) &&
4206  ( _THIS->hessianType != HST_ZERO ) && ( _THIS->hessianType != HST_IDENTITY ) )
4207  {
4208  for( i=0; i<=j+1; ++i )
4209  QProblemB_applyGivens( c,s,nu,RR(i,1+j),RR(i,j), &RR(i,1+j),&RR(i,j) );
4210  }
4211  }
4212 
4213 
4214  if ( nAC > 0 ) /* ( nAC == 0 ) <=> ( nZ == nFR ) <=> Y and T are empty => nothing to do */
4215  {
4216  /* store new column a in a temporary vector instead of shifting T one column to the left */
4217  for( i=0; i<nAC; ++i )
4218  tmp[i] = 0.0;
4219 
4220  {
4221  j = nZ-1;
4222 
4223  QProblemB_computeGivens( w[j+1],w[j], &w[j+1],&w[j],&c,&s );
4224  nu = s/(1.0+c);
4225 
4226  for( i=0; i<nFR; ++i )
4227  {
4228  ii = FR_idx[i];
4229  QProblemB_applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), &QQ(ii,1+j),&QQ(ii,j) );
4230  }
4231 
4232  QProblemB_applyGivens( c,s,nu,TT(nAC-1,tcol),tmp[nAC-1], &tmp[nAC-1],&TT(nAC-1,tcol) );
4233  }
4234 
4235  for( j=nZ; j<nFR-1; ++j )
4236  {
4237  QProblemB_computeGivens( w[j+1],w[j], &w[j+1],&w[j],&c,&s );
4238  nu = s/(1.0+c);
4239 
4240  for( i=0; i<nFR; ++i )
4241  {
4242  ii = FR_idx[i];
4243  QProblemB_applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), &QQ(ii,1+j),&QQ(ii,j) );
4244  }
4245 
4246  for( i=(nFR-2-j); i<nAC; ++i )
4247  QProblemB_applyGivens( c,s,nu,TT(i,1+tcol-nZ+j),tmp[i], &tmp[i],&TT(i,1+tcol-nZ+j) );
4248  }
4249  }
4250 
4251 
4252  if ( ( updateCholesky == BT_TRUE ) &&
4253  ( _THIS->hessianType != HST_ZERO ) && ( _THIS->hessianType != HST_IDENTITY ) )
4254  {
4255  /* IV) RESTORE TRIANGULAR FORM OF R:
4256  * use row-wise Givens rotations to restore upper triangular form of R */
4257  for( i=0; i<nZ-1; ++i )
4258  {
4259  QProblemB_computeGivens( RR(i,i),RR(1+i,i), &RR(i,i),&RR(1+i,i),&c,&s );
4260  nu = s/(1.0+c);
4261 
4262  for( j=(1+i); j<nZ-1; ++j ) /* last column of R is thrown away */
4263  QProblemB_applyGivens( c,s,nu,RR(i,j),RR(1+i,j), &RR(i,j),&RR(1+i,j) );
4264  }
4265  /* last column of R is thrown away */
4266  for( i=0; i<nZ; ++i )
4267  RR(i,nZ-1) = 0.0;
4268  }
4269 
4270 
4271  /* V) UPDATE INDICES */
4272  _THIS->tabularOutput.idxAddB = number;
4273  if ( Bounds_moveFreeToFixed( &(_THIS->bounds),number,B_status ) != SUCCESSFUL_RETURN )
4274  return THROWERROR( RET_ADDBOUND_FAILED );
4275 
4276  return SUCCESSFUL_RETURN;
4277 }
4278 
4279 
4280 /*
4281  * a d d B o u n d _ c h e c k L I
4282  */
4283 returnValue QProblem_addBound_checkLI( QProblem* _THIS, int number )
4284 {
4285  int i, ii;
4286  int nV = QProblem_getNV( _THIS ); /* for QQ() macro */
4287  int nFR = QProblem_getNFR( _THIS );
4288  int nAC = QProblem_getNAC( _THIS );
4289  int nFX = QProblem_getNFX( _THIS );
4290  int nC = QProblem_getNC( _THIS );
4291  returnValue returnvalue = RET_LINEARLY_DEPENDENT;
4292 
4293  myStatic real_t delta_g[NVMAX];
4294  myStatic real_t delta_xFX[NVMAX];
4295  myStatic real_t delta_xFR[NVMAX];
4296  myStatic real_t delta_yAC[NCMAX];
4297  myStatic real_t delta_yFX[NVMAX];
4298 
4299  int dim, nZ;
4300  myStatic real_t nul[NVCMAX];
4301  returnValue dsdReturnValue;
4302 
4303  real_t weight = 0.0;
4304  real_t zero = 0.0;
4305 
4306  if (_THIS->options.enableFullLITests)
4307  {
4308  /*
4309  * expensive LI test. Backsolve with refinement using special right
4310  * hand side. This gives an estimate for what should be considered
4311  * "zero". We then check linear independence relative to _THIS estimate.
4312  */
4313 
4314  /*
4315  * expensive LI test. Backsolve with refinement using special right
4316  * hand side. This gives an estimate for what should be considered
4317  * "zero". We then check linear independence relative to _THIS estimate.
4318  */
4319 
4320  for (ii = 0; ii < nV; ++ii)
4321  delta_g[ii] = 0.0;
4322  delta_g[number] = 1.0; /* sign doesn't matter here */
4323 
4324  dim = (nC>nV)?nC:nV;
4325  for (ii = 0; ii < dim; ++ii)
4326  nul[ii]=0.0;
4327 
4328  dsdReturnValue = QProblem_determineStepDirection( _THIS,
4329  delta_g, nul, nul, nul, nul, BT_FALSE, BT_FALSE,
4330  delta_xFX, delta_xFR, delta_yAC, delta_yFX);
4331  if (dsdReturnValue != SUCCESSFUL_RETURN)
4332  returnvalue = dsdReturnValue;
4333 
4334  /* compute the weight in inf-norm */
4335  for (ii = 0; ii < nAC; ++ii)
4336  {
4337  real_t a = qpOASES_getAbs (delta_yAC[ii]);
4338  if (weight < a) weight = a;
4339  }
4340  for (ii = 0; ii < nFX; ++ii)
4341  {
4342  real_t a = qpOASES_getAbs (delta_yFX[ii]);
4343  if (weight < a) weight = a;
4344  }
4345 
4346  /* look at the "zero" in a relative inf-norm */
4347  for (ii = 0; ii < nFX; ++ii)
4348  {
4349  real_t a = qpOASES_getAbs (delta_xFX[ii]);
4350  if (zero < a) zero = a;
4351  }
4352  for (ii = 0; ii < nFR; ++ii)
4353  {
4354  real_t a = qpOASES_getAbs (delta_xFR[ii]);
4355  if (zero < a) zero = a;
4356  }
4357 
4358  /* relative test against zero in inf-norm */
4359  if (zero > _THIS->options.epsLITests * weight)
4360  returnvalue = RET_LINEARLY_INDEPENDENT;
4361  }
4362  else
4363  {
4364  /*
4365  * cheap LI test for simple bound. Check if constraint <number> is
4366  * linearly independent from the the active ones (<=> is element of null
4367  * space of Afr).
4368  */
4369 
4370  /* some definitions */
4371  nZ = QProblem_getNZ( _THIS );
4372 
4373  for( i=0; i<nZ; ++i )
4374  if ( qpOASES_getAbs( QQ(number,i) ) > _THIS->options.epsLITests )
4375  {
4376  returnvalue = RET_LINEARLY_INDEPENDENT;
4377  break;
4378  }
4379  }
4380 
4381  return THROWINFO( returnvalue );
4382 }
4383 
4384 
4385 /*
4386  * a d d B o u n d _ e n s u r e L I
4387  */
4388 returnValue QProblem_addBound_ensureLI( QProblem* _THIS, int number, SubjectToStatus B_status )
4389 {
4390  int i, ii;
4391  #ifndef __SUPPRESSANYOUTPUT__
4392  myStatic char messageString[QPOASES_MAX_STRING_LENGTH];
4393  #endif
4394 
4395  int nV = QProblem_getNV( _THIS );
4396  int nFX = QProblem_getNFX( _THIS );
4397  int nAC = QProblem_getNAC( _THIS );
4398  int nZ = QProblem_getNZ( _THIS );
4399 
4400  int *FR_idx, *FX_idx, *AC_idx;
4401 
4402  myStatic real_t xiC[NCMAX];
4403  myStatic real_t xiC_TMP[NCMAX];
4404  myStatic real_t xiB[NVMAX];
4405  myStatic real_t num[NVMAX];
4406 
4407  real_t y_min = _THIS->options.maxDualJump;
4408  int y_min_number = -1;
4409  int y_min_number_bound = -1;
4410  BooleanType y_min_isBound = BT_FALSE;
4411 
4412  returnValue returnvalue = SUCCESSFUL_RETURN;
4413 
4414 
4415  /* I) Check if new constraint is linearly independent from the active ones. */
4416  returnValue returnvalueCheckLI = QProblem_addBound_checkLI( _THIS,number );
4417 
4418  if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
4419  return THROWERROR( RET_ENSURELI_FAILED );
4420 
4421  if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
4422  return SUCCESSFUL_RETURN;
4423 
4424 
4425  /* II) NEW BOUND IS LINEARLY DEPENDENT: */
4426  /* 1) Determine coefficients of linear combination,
4427  * cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
4428  * An Algorithm for the Solution of the Parametric Quadratic Programming
4429  * Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
4430  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
4431  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
4432  Indexlist_getNumberArray( Constraints_getActive( &(_THIS->constraints) ),&AC_idx );
4433 
4434  /* 2) Calculate xiC. */
4435  if ( nAC > 0 )
4436  {
4437  if ( B_status == ST_LOWER )
4438  {
4439  for( i=0; i<nAC; ++i )
4440  xiC_TMP[i] = QQ(number,nZ+i);
4441  }
4442  else
4443  {
4444  for( i=0; i<nAC; ++i )
4445  xiC_TMP[i] = -QQ(number,nZ+i);
4446  }
4447 
4448  if ( QProblem_backsolveT( _THIS,xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
4449  {
4450  returnvalue = RET_ENSURELI_FAILED_TQ;
4451  goto farewell;
4452  }
4453  }
4454 
4455  /* 3) Calculate xiB. */
4456  DenseMatrix_subTransTimes(_THIS->A,Constraints_getActive( &(_THIS->constraints)), Bounds_getFixed( &(_THIS->bounds) ), 1, -1.0, xiC, nAC, 0.0, xiB, nFX);
4457 
4458 
4459  /* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
4460 
4461  /* 1) Constraints. */
4462  for( i=0; i<nAC; ++i )
4463  {
4464  ii = AC_idx[i];
4465  num[i] = _THIS->y[nV+ii];
4466  }
4467 
4468  QProblem_performRatioTestC( _THIS,nAC,AC_idx,&(_THIS->constraints), num,xiC, _THIS->options.epsNum,_THIS->options.epsDen, &y_min,&y_min_number );
4469 
4470  /* 2) Bounds. */
4471  for( i=0; i<nFX; ++i )
4472  {
4473  ii = FX_idx[i];
4474  num[i] = _THIS->y[ii];
4475  }
4476 
4477  QProblem_performRatioTestB( _THIS,nFX,FX_idx,&(_THIS->bounds), num,xiB, _THIS->options.epsNum,_THIS->options.epsDen, &y_min,&y_min_number_bound );
4478 
4479  if ( y_min_number_bound >= 0 )
4480  {
4481  y_min_number = y_min_number_bound;
4482  y_min_isBound = BT_TRUE;
4483  }
4484 
4485  /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
4486  if ( y_min_number >= 0 )
4487  {
4488  /* Update Lagrange multiplier... */
4489  for( i=0; i<nAC; ++i )
4490  {
4491  ii = AC_idx[i];
4492  _THIS->y[nV+ii] -= y_min * xiC[i];
4493  }
4494  for( i=0; i<nFX; ++i )
4495  {
4496  ii = FX_idx[i];
4497  _THIS->y[ii] -= y_min * xiB[i];
4498  }
4499 
4500  /* ... also for newly active bound ... */
4501  if ( B_status == ST_LOWER )
4502  _THIS->y[number] = y_min;
4503  else
4504  _THIS->y[number] = -y_min;
4505 
4506  /* ... and for bound to be removed. */
4507  if ( y_min_isBound == BT_TRUE )
4508  {
4509  #ifndef __SUPPRESSANYOUTPUT__
4510  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"bound no. %d.",y_min_number );
4511  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_REMOVE_FROM_ACTIVESET,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
4512  #endif
4513 
4514  if ( QProblem_removeBound( _THIS,y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
4515  {
4516  returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
4517  goto farewell;
4518  }
4519  _THIS->tabularOutput.excRemB = 1;
4520 
4521  _THIS->y[y_min_number] = 0.0;
4522  }
4523  else
4524  {
4525  #ifndef __SUPPRESSANYOUTPUT__
4526  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"constraint no. %d.",y_min_number );
4527  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_REMOVE_FROM_ACTIVESET,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
4528  #endif
4529 
4530  if ( QProblem_removeConstraint( _THIS,y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
4531  {
4532  returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
4533  goto farewell;
4534  }
4535  _THIS->tabularOutput.excRemC = 1;
4536 
4537  _THIS->y[nV+y_min_number] = 0.0;
4538  }
4539  }
4540  else
4541  {
4542  if (_THIS->options.enableDropInfeasibles == BT_TRUE) {
4543  /* dropping of infeasible constraints according to drop priorities */
4544  returnvalue = QProblem_dropInfeasibles( _THIS,number, B_status, BT_TRUE, xiB, xiC);
4545  }
4546  else
4547  {
4548  /* no constraint/bound can be removed => QP is infeasible! */
4549  returnvalue = RET_ENSURELI_FAILED_NOINDEX;
4550  QProblem_setInfeasibilityFlag( _THIS,returnvalue,BT_FALSE );
4551  }
4552  }
4553 
4554 farewell:
4555  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_LI_RESOLVED,0,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
4556 
4557  return ( (returnvalue != SUCCESSFUL_RETURN) && (returnvalue != RET_ENSURELI_FAILED_NOINDEX ) ) ? THROWERROR (returnvalue) : returnvalue;
4558 }
4559 
4560 
4561 
4562 /*
4563  * r e m o v e C o n s t r a i n t
4564  */
4565 returnValue QProblem_removeConstraint( QProblem* _THIS, int number,
4566  BooleanType updateCholesky,
4567  BooleanType allowFlipping,
4568  BooleanType ensureNZC
4569  )
4570 {
4571  int i, j, ii, jj;
4572  returnValue returnvalue = SUCCESSFUL_RETURN;
4573  BooleanType hasFlipped = BT_FALSE;
4574 
4575  /* some definitions */
4576  int nFR = QProblem_getNFR( _THIS );
4577  int nAC = QProblem_getNAC( _THIS );
4578  int nZ = QProblem_getNZ( _THIS );
4579 
4580  int tcol = _THIS->sizeT - nAC;
4581  int number_idx = Indexlist_getIndex( Constraints_getActive( &(_THIS->constraints) ),number );
4582 
4583  int addIdx;
4584  BooleanType addBoundNotConstraint;
4585  SubjectToStatus addStatus;
4586  BooleanType exchangeHappened = BT_FALSE;
4587 
4588  int *FR_idx;
4589 
4590  myStatic real_t Hz[NVMAX];
4591  myStatic real_t z[NVMAX];
4592  real_t rho2 = 0.0;
4593  myStatic real_t ZHz[NVMAX];
4594  myStatic real_t r[NVMAX];
4595 
4596  real_t c, s, nu;
4597 
4598 
4599  /* consistency check */
4600  if ( ( QProblem_getStatus( _THIS ) == QPS_NOTINITIALISED ) ||
4601  ( QProblem_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
4602  ( QProblem_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
4603  ( QProblem_getStatus( _THIS ) == QPS_SOLVED ) )
4604  {
4605  return THROWERROR( RET_UNKNOWN_BUG );
4606  }
4607 
4608  /* consistency checks */
4609  if ( Constraints_getStatus( &(_THIS->constraints),number ) == ST_INACTIVE )
4610  return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
4611 
4612  if ( ( number_idx < 0 ) || ( number_idx >= nAC ) )
4613  return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
4614 
4615 
4616  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
4617 
4618  /* N) PERFORM QPOASES_ZERO CURVATURE TEST. */
4619  if (ensureNZC == BT_TRUE)
4620  {
4621  returnvalue = QProblem_ensureNonzeroCurvature( _THIS,BT_FALSE, number, &exchangeHappened,&addBoundNotConstraint,&addIdx,&addStatus);
4622 
4623  if (returnvalue != SUCCESSFUL_RETURN)
4624  return returnvalue;
4625  }
4626 
4627  /* save index sets and decompositions for flipping bounds strategy */
4628  if ( ( exchangeHappened == BT_FALSE ) && ( _THIS->options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) )
4629  Flipper_set( &(_THIS->flipper),&(_THIS->bounds),_THIS->R,&(_THIS->constraints),_THIS->Q,_THIS->T );
4630 
4631 
4632  /* I) REMOVE <number>th ROW FROM T,
4633  * i.e. shift rows number+1 through nAC upwards (instead of the actual
4634  * constraint number its corresponding index within matrix A is used). */
4635  if ( number_idx < nAC-1 )
4636  {
4637  for( i=(number_idx+1); i<nAC; ++i )
4638  for( j=(nAC-i-1); j<nAC; ++j )
4639  TT(i-1,tcol+j) = TT(i,tcol+j);
4640  /* gimmick: write zeros into the last row of T */
4641  for( j=0; j<nAC; ++j )
4642  TT(nAC-1,tcol+j) = 0.0;
4643 
4644 
4645  /* II) RESTORE TRIANGULAR FORM OF T,
4646  * use column-wise Givens rotations to restore reverse triangular form
4647  * of T simultanenous change of Q (i.e. Y). */
4648 
4649  for( j=(nAC-2-number_idx); j>=0; --j )
4650  {
4651  QProblemB_computeGivens( TT(nAC-2-j,tcol+1+j),TT(nAC-2-j,tcol+j), &TT(nAC-2-j,tcol+1+j),&TT(nAC-2-j,tcol+j),&c,&s );
4652  nu = s/(1.0+c);
4653 
4654  for( i=(nAC-j-1); i<(nAC-1); ++i )
4655  QProblemB_applyGivens( c,s,nu,TT(i,tcol+1+j),TT(i,tcol+j), &TT(i,tcol+1+j),&TT(i,tcol+j) );
4656 
4657  for( i=0; i<nFR; ++i )
4658  {
4659  ii = FR_idx[i];
4660  QProblemB_applyGivens( c,s,nu,QQ(ii,nZ+1+j),QQ(ii,nZ+j), &QQ(ii,nZ+1+j),&QQ(ii,nZ+j) );
4661  }
4662  }
4663  }
4664  else
4665  {
4666  /* gimmick: write zeros into the last row of T */
4667  for( j=0; j<nAC; ++j )
4668  TT(nAC-1,tcol+j) = 0.0;
4669  }
4670 
4671 
4672  if ( ( updateCholesky == BT_TRUE ) &&
4673  ( _THIS->hessianType != HST_ZERO ) && ( _THIS->hessianType != HST_IDENTITY ) )
4674  {
4675  /* III) UPDATE CHOLESKY DECOMPOSITION,
4676  * calculate new additional column (i.e. [r sqrt(rho2)]')
4677  * of the Cholesky factor R. */
4678 
4679  /* 1) Calculate Hz = H*z, where z is the new rightmost column of Z
4680  * (i.e. the old leftmost column of Y). */
4681  for( j=0; j<nFR; ++j )
4682  z[j] = QQ(FR_idx[j],nZ);
4683  DenseMatrix_subTimes(_THIS->H,Bounds_getFree( &(_THIS->bounds) ), Bounds_getFree( &(_THIS->bounds) ), 1, 1.0, z, nFR, 0.0, Hz, nFR, BT_TRUE);
4684 
4685  if ( nZ > 0 )
4686  {
4687  for ( i=0; i<nZ; ++i )
4688  ZHz[i] = 0.0;
4689 
4690  /* 2) Calculate ZHz = Z'*Hz (old Z). */
4691  for( j=0; j<nFR; ++j )
4692  {
4693  jj = FR_idx[j];
4694 
4695  for( i=0; i<nZ; ++i )
4696  ZHz[i] += QQ(jj,i) * Hz[j];
4697  }
4698 
4699  /* 3) Calculate r = R^-T * ZHz. */
4700  if ( QProblem_backsolveR( _THIS,ZHz,BT_TRUE,r ) != SUCCESSFUL_RETURN )
4701  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
4702 
4703  /* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
4704  * and store r into R. */
4705  for( i=0; i<nZ; ++i )
4706  {
4707  rho2 -= r[i]*r[i];
4708  RR(i,nZ) = r[i];
4709  }
4710  }
4711 
4712  /* 5) Store rho into R. */
4713  for( j=0; j<nFR; ++j )
4714  rho2 += QQ(FR_idx[j],nZ) * Hz[j];
4715 
4716  if ( ( _THIS->options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
4717  {
4718  if ( rho2 > _THIS->options.epsFlipping )
4719  RR(nZ,nZ) = qpOASES_getSqrt( rho2 );
4720  else
4721  {
4722  _THIS->hessianType = HST_SEMIDEF;
4723 
4724  Flipper_get( &(_THIS->flipper), &(_THIS->bounds),_THIS->R,&(_THIS->constraints),_THIS->Q,_THIS->T );
4725  Constraints_flipFixed( &(_THIS->constraints),number );
4726  _THIS->tabularOutput.idxAddC = number;
4727  _THIS->tabularOutput.excAddC = 2;
4728 
4729  switch ( Constraints_getStatus( &(_THIS->constraints),number ) )
4730  {
4731  case ST_LOWER:
4732  _THIS->lbA[number] = _THIS->ubA[number]; _THIS->Ax_l[number] = -_THIS->Ax_u[number]; break;
4733  case ST_UPPER:
4734  _THIS->ubA[number] = _THIS->lbA[number]; _THIS->Ax_u[number] = -_THIS->Ax_l[number]; break;
4735  default:
4736  return THROWERROR( RET_MOVING_BOUND_FAILED );
4737  }
4738 
4739  hasFlipped = BT_TRUE;
4740  }
4741  }
4742  else if ( exchangeHappened == BT_FALSE )
4743  {
4744  if ( rho2 > QPOASES_ZERO )
4745  RR(nZ,nZ) = qpOASES_getSqrt( rho2 );
4746  else
4747  {
4748  if ( allowFlipping == BT_FALSE )
4749  {
4750  RR(nZ,nZ) = 100.0*QPOASES_EPS;
4751  }
4752  else
4753  {
4754  _THIS->hessianType = HST_SEMIDEF;
4755  return THROWERROR( RET_HESSIAN_NOT_SPD );
4756  }
4757  }
4758  }
4759  }
4760 
4761 
4762  /* IV) UPDATE INDICES */
4763  _THIS->tabularOutput.idxRemC = number;
4764  if ( hasFlipped == BT_FALSE )
4765  {
4766  if ( Constraints_moveActiveToInactive( &(_THIS->constraints),number ) != SUCCESSFUL_RETURN )
4767  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
4768  }
4769 
4770  if (exchangeHappened == BT_TRUE)
4771  {
4772  /* add bound or constraint */
4773 
4774  /* _THIS->hessianType = HST_SEMIDEF; */
4775  RR(nZ,nZ) = 0.0;
4776 
4777  if ( addBoundNotConstraint )
4778  {
4779  QProblem_addBound( _THIS,addIdx, addStatus, BT_TRUE, BT_FALSE );
4780  _THIS->tabularOutput.excAddB = 1;
4781  }
4782  else
4783  {
4784  QProblem_addConstraint( _THIS,addIdx, addStatus, BT_TRUE, BT_FALSE );
4785  _THIS->tabularOutput.excAddC = 1;
4786  }
4787  }
4788 
4789  return SUCCESSFUL_RETURN;
4790 }
4791 
4792 
4793 /*
4794  * r e m o v e B o u n d
4795  */
4796 returnValue QProblem_removeBound( QProblem* _THIS, int number,
4797  BooleanType updateCholesky,
4798  BooleanType allowFlipping,
4799  BooleanType ensureNZC
4800  )
4801 {
4802  int i, j, ii, jj;
4803  returnValue returnvalue = SUCCESSFUL_RETURN;
4804  int addIdx;
4805  BooleanType addBoundNotConstraint;
4806  SubjectToStatus addStatus;
4807  BooleanType exchangeHappened = BT_FALSE;
4808 
4809  /* some definitions */
4810  int nFR = QProblem_getNFR( _THIS );
4811  int nAC = QProblem_getNAC( _THIS );
4812  int nZ = QProblem_getNZ( _THIS );
4813 
4814  int tcol = _THIS->sizeT - nAC;
4815 
4816  int *FR_idx, *AC_idx;
4817 
4818  int nnFRp1;
4819 
4820  myStatic real_t tmp[NCMAX];
4821  real_t c, s, nu;
4822  real_t z2, rho2;
4823 
4824  myStatic real_t Hz[NVMAX];
4825  myStatic real_t z[NVMAX];
4826 
4827  myStatic real_t r[NVMAX];
4828  myStatic real_t rhs[NVMAX];
4829 
4830 
4831  /* consistency checks */
4832  if ( Bounds_getStatus( &(_THIS->bounds),number ) == ST_INACTIVE )
4833  return THROWERROR( RET_BOUND_NOT_ACTIVE );
4834 
4835  if ( ( QProblem_getStatus( _THIS ) == QPS_NOTINITIALISED ) ||
4836  ( QProblem_getStatus( _THIS ) == QPS_AUXILIARYQPSOLVED ) ||
4837  ( QProblem_getStatus( _THIS ) == QPS_HOMOTOPYQPSOLVED ) ||
4838  ( QProblem_getStatus( _THIS ) == QPS_SOLVED ) )
4839  {
4840  return THROWERROR( RET_UNKNOWN_BUG );
4841  }
4842 
4843  /* 0) PERFORM ZERO CURVATURE TEST. */
4844  if (ensureNZC == BT_TRUE)
4845  {
4846  returnvalue = QProblem_ensureNonzeroCurvature( _THIS,BT_TRUE, number, &exchangeHappened,&addBoundNotConstraint,&addIdx,&addStatus);
4847 
4848  if (returnvalue != SUCCESSFUL_RETURN)
4849  return returnvalue;
4850  }
4851 
4852  /* save index sets and decompositions for flipping bounds strategy */
4853  if ( ( _THIS->options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
4854  Flipper_set( &(_THIS->flipper), &(_THIS->bounds),_THIS->R,&(_THIS->constraints),_THIS->Q,_THIS->T );
4855 
4856  /* I) UPDATE INDICES */
4857  _THIS->tabularOutput.idxRemB = number;
4858  if ( Bounds_moveFixedToFree( &(_THIS->bounds),number ) != SUCCESSFUL_RETURN )
4859  return THROWERROR( RET_REMOVEBOUND_FAILED );
4860 
4861  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
4862 
4863  /* I) APPEND <nFR+1>th UNITY VECTOR TO Q. */
4864  nnFRp1 = FR_idx[nFR];
4865  for( i=0; i<nFR; ++i )
4866  {
4867  ii = FR_idx[i];
4868  QQ(ii,nFR) = 0.0;
4869  QQ(nnFRp1,i) = 0.0;
4870  }
4871  QQ(nnFRp1,nFR) = 1.0;
4872 
4873  if ( nAC > 0 )
4874  {
4875  /* store new column a in a temporary vector instead of shifting T one column to the left and appending a */
4876  Indexlist_getNumberArray( Constraints_getActive( &(_THIS->constraints) ),&AC_idx );
4877 
4878  DenseMatrix_getCol(_THIS->A,number, Constraints_getActive( &(_THIS->constraints)), 1.0, tmp);
4879 
4880 
4881  /* II) RESTORE TRIANGULAR FORM OF T,
4882  * use column-wise Givens rotations to restore reverse triangular form
4883  * of T = [T A(:,number)], simultanenous change of Q (i.e. Y and Z). */
4884  for( j=(nAC-1); j>=0; --j )
4885  {
4886  QProblemB_computeGivens( tmp[nAC-1-j],TT(nAC-1-j,tcol+j), &TT(nAC-1-j,tcol+j),&(tmp[nAC-1-j]),&c,&s );
4887  nu = s/(1.0+c);
4888 
4889  for( i=(nAC-j); i<nAC; ++i )
4890  QProblemB_applyGivens( c,s,nu,tmp[i],TT(i,tcol+j), &TT(i,tcol+j),&(tmp[i]) );
4891 
4892  for( i=0; i<=nFR; ++i )
4893  {
4894  ii = FR_idx[i];
4895  /* nZ+1+nAC = nFR+1 / nZ+(1) = nZ+1 */
4896  QProblemB_applyGivens( c,s,nu,QQ(ii,nZ+1+j),QQ(ii,nZ+j), &QQ(ii,nZ+1+j),&QQ(ii,nZ+j) );
4897  }
4898  }
4899  }
4900 
4901 
4902  if ( ( updateCholesky == BT_TRUE ) &&
4903  ( _THIS->hessianType != HST_ZERO ) && ( _THIS->hessianType != HST_IDENTITY ) )
4904  {
4905  /* III) UPDATE CHOLESKY DECOMPOSITION,
4906  * calculate new additional column (i.e. [r sqrt(rho2)]')
4907  * of the Cholesky factor R: */
4908  z2 = QQ(nnFRp1,nZ);
4909  rho2 = DenseMatrix_diag(_THIS->H,nnFRp1)*z2*z2; /* rho2 = h2*z2*z2 */
4910 
4911  if ( nFR > 0 )
4912  {
4913  /* Attention: Index list of free variables has already grown by one! */
4914  /* 1) Calculate R'*r = Zfr'*Hfr*z1 + z2*Zfr'*h1 =: Zfr'*Hz + z2*Zfr'*h1 =: rhs and
4915  * rho2 = z1'*Hfr*z1 + 2*z2*h1'*z1 + h2*z2^2 - r'*r =: z1'*Hz + 2*z2*h1'*z1 + h2*z2^2 - r'r */
4916  for( j=0; j<nFR; ++j )
4917  z[j] = QQ(FR_idx[j],nZ);
4918  z[nFR] = 0.0;
4919  DenseMatrix_subTimes(_THIS->H,Bounds_getFree( &(_THIS->bounds) ), Bounds_getFree( &(_THIS->bounds) ), 1, 1.0, z, nFR+1, 0.0, Hz, nFR+1, BT_TRUE);
4920 
4921  DenseMatrix_getCol(_THIS->H,nnFRp1, Bounds_getFree( &(_THIS->bounds) ), 1.0, z);
4922 
4923  if ( nZ > 0 )
4924  {
4925  for( i=0; i<nZ; ++i )
4926  rhs[i] = 0.0;
4927 
4928  /* 2) Calculate rhs. */
4929  for( j=0; j<nFR; ++j )
4930  {
4931  jj = FR_idx[j];
4932  for( i=0; i<nZ; ++i )
4933  /* Zfr' * ( Hz + z2*h1 ) */
4934  rhs[i] += QQ(jj,i) * ( Hz[j] + z2 * z[j] );
4935  }
4936 
4937  /* 3) Calculate r = R^-T * rhs. */
4938  if ( QProblem_backsolveRrem( _THIS,rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
4939  return THROWERROR( RET_REMOVEBOUND_FAILED );
4940 
4941 
4942  /* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
4943  * and store r into R. */
4944  for( i=0; i<nZ; ++i )
4945  {
4946  rho2 -= r[i]*r[i];
4947  RR(i,nZ) = r[i];
4948  }
4949  }
4950 
4951  for( j=0; j<nFR; ++j )
4952  {
4953  jj = FR_idx[j];
4954  /* z1' * ( Hz + 2*z2*h1 ) */
4955  rho2 += QQ(jj,nZ) * ( Hz[j] + 2.0*z2*z[j] );
4956  }
4957  }
4958 
4959  /* 5) Store rho into R. */
4960  if ( ( _THIS->options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
4961  {
4962  if ( rho2 > _THIS->options.epsFlipping )
4963  RR(nZ,nZ) = qpOASES_getSqrt( rho2 );
4964  else
4965  {
4966  if ( _THIS->hessianType != HST_ZERO )
4967  _THIS->hessianType = HST_SEMIDEF;
4968 
4969  Flipper_get( &(_THIS->flipper), &(_THIS->bounds),_THIS->R,&(_THIS->constraints),_THIS->Q,_THIS->T );
4970  Bounds_flipFixed( &(_THIS->bounds),number );
4971  _THIS->tabularOutput.idxAddB = number;
4972  _THIS->tabularOutput.excAddB = 2;
4973 
4974  switch ( Bounds_getStatus( &(_THIS->bounds),number) )
4975  {
4976  case ST_LOWER:
4977  _THIS->lb[number] = _THIS->ub[number];
4978  break;
4979  case ST_UPPER:
4980  _THIS->ub[number] = _THIS->lb[number];
4981  break;
4982  default: return THROWERROR( RET_MOVING_BOUND_FAILED );
4983  }
4984 
4985  }
4986  }
4987  else if ( exchangeHappened == BT_FALSE )
4988  {
4989  if ( rho2 > QPOASES_ZERO )
4990  RR(nZ,nZ) = qpOASES_getSqrt( rho2 );
4991  else
4992  {
4993  if ( allowFlipping == BT_FALSE )
4994  RR(nZ,nZ) = 100.0*QPOASES_EPS;
4995  else
4996  {
4997  _THIS->hessianType = HST_SEMIDEF;
4998  return THROWERROR( RET_HESSIAN_NOT_SPD );
4999  }
5000  }
5001  }
5002  else
5003  {
5004  /* add bound or constraint */
5005 
5006  /* _THIS->hessianType = HST_SEMIDEF; */
5007  RR(nZ,nZ) = 0.0;
5008 
5009  if ( addBoundNotConstraint )
5010  {
5011  QProblem_addBound( _THIS,addIdx, addStatus, BT_TRUE, BT_FALSE);
5012  _THIS->tabularOutput.excAddB = 1;
5013  }
5014  else
5015  {
5016  QProblem_addConstraint(_THIS,addIdx, addStatus, BT_TRUE, BT_FALSE);
5017  _THIS->tabularOutput.excAddC = 1;
5018  }
5019  }
5020  }
5021 
5022  return SUCCESSFUL_RETURN;
5023 }
5024 
5025 
5026 
5027 returnValue QProblem_performPlainRatioTest( QProblem* _THIS,
5028  int nIdx,
5029  const int* const idxList,
5030  const real_t* const num,
5031  const real_t* const den,
5032  real_t epsNum,
5033  real_t epsDen,
5034  real_t* t,
5035  int* BC_idx
5036  )
5037 {
5038  int i;
5039  for (i = 0; i < nIdx; i++)
5040  if ( (num[i] > epsNum) && (den[i] > epsDen) && ((*t) * den[i] > num[i]) )
5041  {
5042  *t = num[i] / den[i];
5043  *BC_idx = idxList[i];
5044  }
5045 
5046  return SUCCESSFUL_RETURN;
5047 }
5048 
5049 
5050 returnValue QProblem_ensureNonzeroCurvature( QProblem* _THIS,
5051  BooleanType removeBoundNotConstraint,
5052  int remIdx,
5053  BooleanType* exchangeHappened,
5054  BooleanType* addBoundNotConstraint,
5055  int* addIdx,
5056  SubjectToStatus* addStatus
5057  )
5058 {
5059  int i, ii;
5060  int addLBndIdx = -1, addLCnstrIdx = -1, addUBndIdx = -1, addUCnstrIdx = -1; /* exchange indices */
5061  int *FX_idx, *AC_idx, *IAC_idx, *FR_idx;
5062  returnValue returnvalue = SUCCESSFUL_RETURN;
5063 
5064  int nV = QProblem_getNV( _THIS );
5065  int nFR = QProblem_getNFR( _THIS );
5066  int nAC = QProblem_getNAC( _THIS );
5067  int nC = QProblem_getNC( _THIS );
5068  int nFX = QProblem_getNFX( _THIS );
5069  int nIAC = QProblem_getNIAC( _THIS );
5070 
5071  myStatic real_t delta_xFX[NVMAX];
5072  myStatic real_t delta_xFR[NVMAX];
5073  myStatic real_t delta_yAC[NCMAX];
5074  myStatic real_t delta_yFX[NVMAX];
5075 
5076  int dim;
5077  myStatic real_t nul[NVCMAX];
5078  myStatic real_t ek[NVMAX]; /* minus e_k (bound k is removed) */
5079 
5080  real_t one = 1.0;
5081 
5082  real_t a;
5083  real_t normXi = 0.0;
5084  real_t normS = 0.0;
5085  real_t sigmaLBnd, sigmaLCnstr, sigmaUBnd, sigmaUCnstr, sigma;
5086  myStatic real_t x_W[NVMAX];
5087 
5088  myStatic real_t As[NCMAX];
5089  myStatic real_t Ax_W[NCMAX];
5090 
5091  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
5092  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
5093  Indexlist_getNumberArray( Constraints_getActive( &(_THIS->constraints) ),&AC_idx );
5094  Indexlist_getNumberArray( Constraints_getInactive( &(_THIS->constraints) ),&IAC_idx );
5095 
5096  *addBoundNotConstraint = BT_TRUE;
5097  *addStatus = ST_INACTIVE;
5098  *exchangeHappened = BT_FALSE;
5099 
5100  if (removeBoundNotConstraint)
5101  {
5102  dim = nV < nC ? nC : nV;
5103  for (ii = 0; ii < dim; ++ii)
5104  nul[ii]=0.0;
5105  for (ii = 0; ii < nV; ++ii)
5106  ek[ii]=0.0;
5107  ek[remIdx] = Bounds_getStatus( &(_THIS->bounds),remIdx) == ST_LOWER ? 1.0 : -1.0;
5108 
5109  returnvalue = QProblem_determineStepDirection( _THIS,nul, nul, nul, ek, ek,
5110  BT_FALSE, BT_FALSE,
5111  delta_xFX, delta_xFR, delta_yAC, delta_yFX
5112  );
5113  }
5114  else
5115  {
5116  for (ii = 0; ii < nV; ++ii)
5117  nul[ii]=0.0;
5118  for (ii = 0; ii < nC; ++ii)
5119  ek[ii]=0.0;
5120  ek[remIdx] = Constraints_getStatus( &(_THIS->constraints),remIdx) == ST_LOWER ? 1.0 : -1.0;
5121 
5122  returnvalue = QProblem_determineStepDirection( _THIS,nul,
5123  ek, ek, nul, nul,
5124  BT_FALSE, BT_TRUE,
5125  delta_xFX, delta_xFR, delta_yAC, delta_yFX
5126  );
5127  }
5128 
5129  /* compute the weight in inf-norm */
5130  for (ii = 0; ii < nAC; ++ii)
5131  {
5132  a = qpOASES_getAbs (delta_yAC[ii]);
5133  if (normXi < a) normXi = a;
5134  }
5135  for (ii = 0; ii < nFX; ++ii)
5136  {
5137  a = qpOASES_getAbs (delta_yFX[ii]);
5138  if (normXi < a) normXi = a;
5139  }
5140 
5141  /* look at the "zero" in a relative inf-norm */
5142  for (ii = 0; ii < nFX; ++ii)
5143  {
5144  a = qpOASES_getAbs (delta_xFX[ii]);
5145  if (normS < a) normS = a;
5146  }
5147  for (ii = 0; ii < nFR; ++ii)
5148  {
5149  a = qpOASES_getAbs (delta_xFR[ii]);
5150  if (normS < a) normS = a;
5151  }
5152 
5153  /* relative test against zero in inf-norm */
5154  if (normXi < _THIS->options.epsNZCTests * normS)
5155  {
5156  /* determine jump in x via ratio tests */
5157 
5158  /* bounds */
5159 
5160  /* compress x-u */
5161  for (i = 0; i < nFR; i++)
5162  {
5163  ii = FR_idx[i];
5164  x_W[i] = _THIS->ub[ii] - _THIS->x[ii];
5165  }
5166  /* performRatioTest( nFR,FR_idx,&(_THIS->bounds),x_W,delta_xFR, _THIS->options.epsNum,_THIS->options.epsDen, sigmaUBnd,addUBndIdx ); */
5167  sigmaUBnd = _THIS->options.maxPrimalJump;
5168  addUBndIdx = -1;
5169  QProblem_performPlainRatioTest(_THIS,nFR, FR_idx, x_W, delta_xFR, _THIS->options.epsNum, _THIS->options.epsDen, &sigmaUBnd,&addUBndIdx);
5170  if (removeBoundNotConstraint == BT_TRUE && Bounds_getStatus( &(_THIS->bounds),remIdx) == ST_LOWER)
5171  {
5172  /* also consider bound which is to be removed */
5173  one = 1.0;
5174  x_W[0] = _THIS->ub[remIdx] - _THIS->x[remIdx];
5175  QProblem_performPlainRatioTest(_THIS,1, &remIdx, x_W, &one, _THIS->options.epsNum, _THIS->options.epsDen, &sigmaUBnd,&addUBndIdx);
5176  }
5177 
5178  /* compress x-l */
5179  for (i = 0; i < nFR; i++)
5180  {
5181  ii = FR_idx[i];
5182  x_W[i] = _THIS->x[ii] - _THIS->lb[ii];
5183  }
5184  for (i = 0; i < nFR; i++)
5185  delta_xFR[i] = -delta_xFR[i];
5186  /* performRatioTest( nFR,FR_idx,&(_THIS->bounds),x_W,delta_xFR, _THIS->options.epsNum,_THIS->options.epsDen, sigmaLBnd,addLBndIdx ); */
5187  sigmaLBnd = _THIS->options.maxPrimalJump;
5188  addLBndIdx = -1;
5189  QProblem_performPlainRatioTest(_THIS,nFR, FR_idx, x_W, delta_xFR, _THIS->options.epsNum, _THIS->options.epsDen, &sigmaLBnd,&addLBndIdx);
5190  if (removeBoundNotConstraint == BT_TRUE && Bounds_getStatus( &(_THIS->bounds),remIdx) == ST_UPPER)
5191  {
5192  /* also consider bound which is to be removed */
5193  one = 1.0;
5194  x_W[0] = _THIS->x[remIdx] - _THIS->lb[remIdx];
5195  QProblem_performPlainRatioTest(_THIS,1, &remIdx, x_W, &one, _THIS->options.epsNum, _THIS->options.epsDen, &sigmaLBnd,&addLBndIdx);
5196  }
5197  for (i = 0; i < nFR; i++)
5198  delta_xFR[i] = -delta_xFR[i];
5199 
5200  /* constraints */
5201 
5202  /* compute As (compressed to inactive constraints) */
5203  DenseMatrix_subTimes(_THIS->A,Constraints_getInactive(&(_THIS->constraints)), Bounds_getFixed( &(_THIS->bounds) ), 1, 1.0, delta_xFX, nFX, 0.0, As, nIAC, BT_TRUE);
5204  DenseMatrix_subTimes(_THIS->A,Constraints_getInactive(&(_THIS->constraints)), Bounds_getFree( &(_THIS->bounds) ), 1, 1.0, delta_xFR, nFR, 1.0, As, nIAC, BT_TRUE);
5205 
5206  /* compress Ax_u */
5207  for (i = 0; i < nIAC; i++)
5208  {
5209  ii = IAC_idx[i];
5210  Ax_W[i] = _THIS->Ax_u[ii];
5211  }
5212  /* performRatioTest( nIAC,IAC_idx,&(_THIS->constraints), Ax_W,As, _THIS->options.epsNum,_THIS->options.epsDen, sigmaUCnstr,addUCnstrIdx ); */
5213  sigmaUCnstr = _THIS->options.maxPrimalJump;
5214  addUCnstrIdx = -1;
5215  QProblem_performPlainRatioTest(_THIS,nIAC, IAC_idx, Ax_W, As, _THIS->options.epsNum, _THIS->options.epsDen, &sigmaUCnstr,&addUCnstrIdx);
5216  if (removeBoundNotConstraint == BT_FALSE && Constraints_getStatus( &(_THIS->constraints),remIdx) == ST_LOWER)
5217  {
5218  /* also consider constraint which is to be removed */
5219  one = 1.0;
5220  QProblem_performPlainRatioTest(_THIS,1, &remIdx, &_THIS->Ax_u[remIdx], &one, _THIS->options.epsNum, _THIS->options.epsDen, &sigmaUCnstr,&addUCnstrIdx);
5221  }
5222 
5223  /* compress Ax_l */
5224  for (i = 0; i < nIAC; i++)
5225  {
5226  ii = IAC_idx[i];
5227  Ax_W[i] = _THIS->Ax_l[ii];
5228  }
5229  for (i = 0; i < nIAC; i++)
5230  As[i] = -As[i];
5231  /* performRatioTest( nIAC,IAC_idx,&(_THIS->constraints), Ax_W,As, _THIS->options.epsNum,_THIS->options.epsDen, sigmaLCnstr,addLCnstrIdx ); */
5232  sigmaLCnstr = _THIS->options.maxPrimalJump;
5233  addLCnstrIdx = -1;
5234  QProblem_performPlainRatioTest(_THIS,nIAC, IAC_idx, Ax_W, As, _THIS->options.epsNum, _THIS->options.epsDen, &sigmaLCnstr,&addLCnstrIdx);
5235  if (removeBoundNotConstraint == BT_FALSE && Constraints_getStatus( &(_THIS->constraints),remIdx) == ST_UPPER)
5236  {
5237  /* also consider constraint which is to be removed */
5238  one = 1.0;
5239  QProblem_performPlainRatioTest(_THIS,1, &remIdx, &_THIS->Ax_l[remIdx], &one, _THIS->options.epsNum, _THIS->options.epsDen, &sigmaLCnstr,&addLCnstrIdx);
5240  }
5241 
5242  /* perform primal jump */
5243  sigma = _THIS->options.maxPrimalJump;
5244  if (sigmaUCnstr < sigma) { sigma = sigmaUCnstr; *addStatus = ST_UPPER; *addBoundNotConstraint = BT_FALSE; *addIdx = addUCnstrIdx; }
5245  if (sigmaLCnstr < sigma) { sigma = sigmaLCnstr; *addStatus = ST_LOWER; *addBoundNotConstraint = BT_FALSE; *addIdx = addLCnstrIdx; }
5246  if (sigmaUBnd < sigma) { sigma = sigmaUBnd; *addStatus = ST_UPPER; *addBoundNotConstraint = BT_TRUE; *addIdx = addUBndIdx; }
5247  if (sigmaLBnd < sigma) { sigma = sigmaLBnd; *addStatus = ST_LOWER; *addBoundNotConstraint = BT_TRUE; *addIdx = addLBndIdx; }
5248 
5249  if (sigma >= _THIS->options.maxPrimalJump)
5250  {
5251  _THIS->unbounded = BT_TRUE;
5252  returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
5253  }
5254  else
5255  {
5256  for (i = 0; i < nFR; i++)
5257  _THIS->x[FR_idx[i]] += sigma * delta_xFR[i];
5258 
5259  for (i = 0; i < nFX; i++)
5260  _THIS->x[FX_idx[i]] += sigma * delta_xFX[i];
5261 
5262  /* update Ax, Ax_u, and Ax_l */
5263  DenseMatrix_times(_THIS->A,1, 1.0, _THIS->x, nV, 0.0, _THIS->Ax, nC);
5264  for (i = 0; i < nC; i++) _THIS->Ax_u[i] = _THIS->ubA[i] - _THIS->Ax[i];
5265  for (i = 0; i < nC; i++) _THIS->Ax_l[i] = _THIS->Ax[i] - _THIS->lbA[i];
5266 
5267  /* change working set later */
5268  *exchangeHappened = BT_TRUE;
5269  }
5270  }
5271 
5272  return returnvalue;
5273 }
5274 
5275 
5276 
5277 /*
5278  * b a c k s o l v e T
5279  */
5280 returnValue QProblem_backsolveT( QProblem* _THIS, const real_t* const b, BooleanType transposed, real_t* const a )
5281 {
5282  int i, j;
5283  int nT = QProblem_getNAC( _THIS );
5284  int tcol = _THIS->sizeT - nT;
5285 
5286  real_t sum;
5287 
5288  /* nothing to do */
5289  if ( nT <= 0 )
5290  return SUCCESSFUL_RETURN;
5291 
5292 
5293  /* Solve Ta = b, where T might be transposed. */
5294  if ( transposed == BT_FALSE )
5295  {
5296  /* solve Ta = b */
5297  for( i=0; i<nT; ++i )
5298  {
5299  sum = b[i];
5300  for( j=0; j<i; ++j )
5301  sum -= TT(i,_THIS->sizeT-1-j) * a[nT-1-j];
5302 
5303  if ( qpOASES_getAbs( TT(i,_THIS->sizeT-1-i) ) > QPOASES_EPS )
5304  a[nT-1-i] = sum / TT(i,_THIS->sizeT-1-i);
5305  else
5306  return THROWERROR( RET_DIV_BY_ZERO );
5307  }
5308  }
5309  else
5310  {
5311  /* solve T^T*a = b */
5312  for( i=0; i<nT; ++i )
5313  {
5314  sum = b[i];
5315  for( j=0; j<i; ++j )
5316  sum -= TT(nT-1-j,tcol+i) * a[nT-1-j];
5317 
5318  if ( qpOASES_getAbs( TT(nT-1-i,tcol+i) ) > QPOASES_EPS )
5319  a[nT-1-i] = sum / TT(nT-1-i,tcol+i);
5320  else
5321  return THROWERROR( RET_DIV_BY_ZERO );
5322  }
5323  }
5324 
5325 
5326  return SUCCESSFUL_RETURN;
5327 }
5328 
5329 
5330 /*
5331  * d e t e r m i n e D a t a S h i f t
5332  */
5333 returnValue QProblem_determineDataShift( QProblem* _THIS, const real_t* const g_new, const real_t* const lbA_new, const real_t* const ubA_new,
5334  const real_t* const lb_new, const real_t* const ub_new,
5335  real_t* const delta_g, real_t* const delta_lbA, real_t* const delta_ubA,
5336  real_t* const delta_lb, real_t* const delta_ub,
5337  BooleanType* Delta_bC_isZero, BooleanType* Delta_bB_isZero
5338  )
5339 {
5340  int i, ii;
5341  int nC = QProblem_getNC( _THIS );
5342  int nAC = QProblem_getNAC( _THIS );
5343 
5344  int* AC_idx;
5345 
5346  Indexlist_getNumberArray( Constraints_getActive( &(_THIS->constraints) ),&AC_idx );
5347 
5348 
5349 
5350  /* I) DETERMINE DATA SHIFT FOR BOUNDS */
5351  QProblemBCPY_determineDataShift( _THIS,g_new,lb_new,ub_new,
5352  delta_g,delta_lb,delta_ub,
5353  Delta_bB_isZero );
5354 
5355 
5356  /* II) DETERMINE DATA SHIFT FOR CONSTRAINTS */
5357  /* 1) Calculate shift directions. */
5358  for( i=0; i<nC; ++i )
5359  {
5360  /* if lower constraints' bounds are to be disabled or do not exist, shift them to -infinity */
5361  if ( lbA_new != 0 )
5362  delta_lbA[i] = lbA_new[i] - _THIS->lbA[i];
5363  else
5364  delta_lbA[i] = -QPOASES_INFTY - _THIS->lbA[i];
5365  }
5366 
5367  for( i=0; i<nC; ++i )
5368  {
5369  /* if upper constraints' bounds are to be disabled or do not exist, shift them to infinity */
5370  if ( ubA_new != 0 )
5371  delta_ubA[i] = ubA_new[i] - _THIS->ubA[i];
5372  else
5373  delta_ubA[i] = QPOASES_INFTY - _THIS->ubA[i];
5374  }
5375 
5376  /* 2) Determine if active constraints' bounds are to be shifted. */
5377  *Delta_bC_isZero = BT_TRUE;
5378 
5379  for ( i=0; i<nAC; ++i )
5380  {
5381  ii = AC_idx[i];
5382 
5383  if ( ( qpOASES_getAbs( delta_lbA[ii] ) > QPOASES_EPS ) || ( qpOASES_getAbs( delta_ubA[ii] ) > QPOASES_EPS ) )
5384  {
5385  *Delta_bC_isZero = BT_FALSE;
5386  break;
5387  }
5388  }
5389 
5390  return SUCCESSFUL_RETURN;
5391 }
5392 
5393 
5394 /*
5395  * d e t e r m i n e S t e p D i r e c t i o n
5396  */
5397 returnValue QProblem_determineStepDirection( QProblem* _THIS, const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
5398  const real_t* const delta_lb, const real_t* const delta_ub,
5399  BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
5400  real_t* const delta_xFX, real_t* const delta_xFR,
5401  real_t* const delta_yAC, real_t* const delta_yFX
5402  )
5403 {
5404  int i, j, ii, jj, r;
5405  int nFR = QProblem_getNFR( _THIS );
5406  int nFX = QProblem_getNFX( _THIS );
5407  int nAC = QProblem_getNAC( _THIS );
5408  int nZ = QProblem_getNZ( _THIS );
5409 
5410  int* FR_idx;
5411  int* FX_idx;
5412  int* AC_idx;
5413 
5414  real_t rnrm = 0.0;
5415 
5416  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
5417  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
5418  Indexlist_getNumberArray( Constraints_getActive( &(_THIS->constraints) ),&AC_idx );
5419 
5420 
5421  /* I) DETERMINE delta_xFX (_THIS is exact, does not need refinement) */
5422  if ( Delta_bB_isZero == BT_FALSE )
5423  {
5424  for( i=0; i<nFX; ++i )
5425  {
5426  ii = FX_idx[i];
5427 
5428  if ( Bounds_getStatus( &(_THIS->bounds),ii ) == ST_LOWER )
5429  delta_xFX[i] = delta_lb[ii];
5430  else
5431  delta_xFX[i] = delta_ub[ii];
5432  }
5433  }
5434  else
5435  {
5436  for( i=0; i<nFX; ++i )
5437  delta_xFX[i] = 0.0;
5438  }
5439 
5440 
5441  /* tempA and tempB hold the residuals in gFR and bA (= lbA or ubA)
5442  * delta_xFR, delta_yAC hold the steps that get refined */
5443  for ( i=0; i<nFR; ++i )
5444  {
5445  ii = FR_idx[i];
5446  _THIS->tempA[i] = delta_g[ii];
5447  delta_xFR[i] = 0.0;
5448  }
5449  for ( i=0; i<nAC; ++i )
5450  delta_yAC[i] = 0.0;
5451  if ( Delta_bC_isZero == BT_FALSE )
5452  {
5453  for ( i=0; i<nAC; ++i )
5454  {
5455  ii = AC_idx[i];
5456  if ( Constraints_getStatus( &(_THIS->constraints),ii ) == ST_LOWER )
5457  _THIS->tempB[i] = delta_lbA[ii];
5458  else
5459  _THIS->tempB[i] = delta_ubA[ii];
5460  }
5461  }
5462  else
5463  {
5464  for ( i=0; i<nAC; ++i )
5465  _THIS->tempB[i] = 0.0;
5466  }
5467 
5468  /* Iterative refinement loop for delta_xFRz, delta_xFRy, delta_yAC */
5469  for ( r=0; r<=_THIS->options.numRefinementSteps; ++r )
5470  {
5471  /* II) DETERMINE delta_xFR */
5472  if ( nFR > 0 )
5473  {
5474  for( i=0; i<nFR; ++i )
5475  _THIS->delta_xFR_TMP[i] = 0.0;
5476 
5477  /* 1) Determine delta_xFRy. */
5478  if ( nAC > 0 )
5479  {
5480  if ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) )
5481  {
5482  for( i=0; i<nAC; ++i )
5483  _THIS->delta_xFRy[i] = 0.0;
5484  }
5485  else
5486  {
5487  /* compute bA - A * delta_xFX. tempB already holds bA->
5488  * in refinements r>=1, delta_xFX is exactly zero */
5489  if ( ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
5490  DenseMatrix_subTimes(_THIS->A,Constraints_getActive( &(_THIS->constraints)), Bounds_getFixed( &(_THIS->bounds) ), 1, -1.0, delta_xFX, nFX, 1.0, _THIS->tempB, nAC, BT_TRUE);
5491 
5492  if ( QProblem_backsolveT( _THIS,_THIS->tempB, BT_FALSE, _THIS->delta_xFRy ) != SUCCESSFUL_RETURN )
5493  return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
5494 
5495  for( i=0; i<nFR; ++i )
5496  {
5497  ii = FR_idx[i];
5498  for( j=0; j<nAC; ++j )
5499  _THIS->delta_xFR_TMP[i] += QQ(ii,nZ+j) * _THIS->delta_xFRy[j];
5500  }
5501  }
5502  }
5503 
5504 
5505  /* 2) Determine delta_xFRz. */
5506  for( i=0; i<nZ; ++i )
5507  _THIS->delta_xFRz[i] = 0.0;
5508 
5509  if ( ( _THIS->hessianType == HST_ZERO ) || ( _THIS->hessianType == HST_IDENTITY ) )
5510  {
5511  /* compute Z*delta_gFR [/eps] (delta_gFR is stored in tempA) */
5512  for( j=0; j<nFR; ++j )
5513  {
5514  jj = FR_idx[j];
5515  for( i=0; i<nZ; ++i )
5516  _THIS->delta_xFRz[i] -= QQ(jj,i) * _THIS->tempA[j];
5517  }
5518 
5519  if ( _THIS->hessianType == HST_ZERO )
5520  {
5521  if ( QProblem_usingRegularisation( _THIS ) == BT_TRUE )
5522  {
5523  for( i=0; i<nZ; ++i )
5524  _THIS->delta_xFRz[i] /= _THIS->regVal;
5525  }
5526  else
5527  {
5528  /* When solving LPs without regularisation, iterates must always be at a vertex. */
5529  if ( nZ > 0 )
5530  return THROWERROR( RET_UNKNOWN_BUG );
5531  }
5532  }
5533  }
5534  else
5535  {
5536  /* compute HMX*delta_xFX. DESTROY delta_gFR that was in tempA */
5537  if ( ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
5538  DenseMatrix_subTimes(_THIS->H,Bounds_getFree( &(_THIS->bounds) ), Bounds_getFixed( &(_THIS->bounds) ), 1, 1.0, delta_xFX, nFX, 1.0, _THIS->tempA, nFR, BT_TRUE);
5539 
5540  /* compute HFR*delta_xFRy */
5541  if ( ( nAC > 0 ) && ( ( Delta_bC_isZero == BT_FALSE ) || ( Delta_bB_isZero == BT_FALSE ) ) )
5542  DenseMatrix_subTimes(_THIS->H,Bounds_getFree( &(_THIS->bounds) ), Bounds_getFree( &(_THIS->bounds) ), 1, 1.0, _THIS->delta_xFR_TMP, nFR, 1.0, _THIS->tempA, nFR, BT_TRUE);
5543 
5544  /* compute ZFR_delta_xFRz = (Z'*HFR*Z) \ Z * (HFR*delta_xFR + HMX*delta_xFX + delta_gFR) */
5545  if ( nZ > 0 )
5546  {
5547  for( j=0; j<nFR; ++j )
5548  {
5549  jj = FR_idx[j];
5550  for( i=0; i<nZ; ++i )
5551  _THIS->delta_xFRz[i] -= QQ(jj,i) * _THIS->tempA[j];
5552  }
5553 
5554  if ( QProblem_backsolveR( _THIS,_THIS->delta_xFRz,BT_TRUE,_THIS->delta_xFRz ) != SUCCESSFUL_RETURN )
5555  return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
5556 
5557  if ( QProblem_backsolveR( _THIS,_THIS->delta_xFRz,BT_FALSE,_THIS->delta_xFRz ) != SUCCESSFUL_RETURN )
5558  return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
5559  }
5560  }
5561 
5562  /* compute Z * ZFR_delta_xFRz */
5563  if ( nZ > 0 )
5564  {
5565  for( i=0; i<nFR; ++i )
5566  {
5567  _THIS->ZFR_delta_xFRz[i] = 0.0;
5568 
5569  ii = FR_idx[i];
5570  for( j=0; j<nZ; ++j )
5571  _THIS->ZFR_delta_xFRz[i] += QQ(ii,j) * _THIS->delta_xFRz[j];
5572 
5573  _THIS->delta_xFR_TMP[i] += _THIS->ZFR_delta_xFRz[i];
5574  }
5575  }
5576  }
5577 
5578  /* III) DETERMINE delta_yAC */
5579  if ( nAC > 0 ) /* => ( nFR = nZ + nAC > 0 ) */
5580  {
5581  if ( ( _THIS->hessianType == HST_ZERO ) || ( _THIS->hessianType == HST_IDENTITY ) )
5582  {
5583  /* if zero: delta_yAC = (T')^-1 * ( Yfr*delta_gFR + eps*delta_xFRy ),
5584  * if identity: delta_yAC = (T')^-1 * ( Yfr*delta_gFR + delta_xFRy )
5585  *
5586  * DESTROY residual_bA that was stored in tempB
5587  * If we come here, residual_gFR in tempA is STILL VALID
5588  */
5589  if ( _THIS->hessianType == HST_IDENTITY )
5590  {
5591  for( j=0; j<nAC; ++j )
5592  _THIS->tempB[j] = _THIS->delta_xFRy[j];
5593  }
5594  else /* hessianType == HST_ZERO */
5595  {
5596  if ( QProblem_usingRegularisation( _THIS ) == BT_TRUE )
5597  {
5598  for( j=0; j<nAC; ++j )
5599  _THIS->tempB[j] = _THIS->regVal * _THIS->delta_xFRy[j];
5600  }
5601  else
5602  {
5603  for( j=0; j<nAC; ++j )
5604  _THIS->tempB[j] = 0.0;
5605  }
5606  }
5607 
5608  for( j=0; j<nAC; ++j )
5609  {
5610  for( i=0; i<nFR; ++i )
5611  {
5612  ii = FR_idx[i];
5613  _THIS->tempB[j] += QQ(ii,nZ+j) * _THIS->tempA[i];
5614  }
5615  }
5616  }
5617  else
5618  {
5619  /* Compute HFR * delta_xFR + HMX*delta_xFX
5620  * Here, tempA holds (HFR*delta_xFRy + HMX*delta_xFX) */
5621  if ( nZ > 0 )
5622  DenseMatrix_subTimes(_THIS->H,Bounds_getFree( &(_THIS->bounds) ), Bounds_getFree( &(_THIS->bounds) ), 1, 1.0, _THIS->ZFR_delta_xFRz, nFR, 1.0, _THIS->tempA, nFR, BT_TRUE);
5623 
5624  for( i=0; i<nAC; ++i)
5625  {
5626  _THIS->tempB[i] = 0.0;
5627  for( j=0; j<nFR; ++j )
5628  {
5629  jj = FR_idx[j];
5630  _THIS->tempB[i] += QQ(jj,nZ+i) * _THIS->tempA[j];
5631  }
5632  }
5633  }
5634 
5635  if ( QProblem_backsolveT( _THIS,_THIS->tempB,BT_TRUE,_THIS->delta_yAC_TMP ) != SUCCESSFUL_RETURN )
5636  return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
5637  }
5638 
5639  /* refine the solution found so far */
5640  for ( i=0; i<nFR; ++i )
5641  delta_xFR[i] += _THIS->delta_xFR_TMP[i];
5642  for ( i=0; i<nAC; ++i )
5643  delta_yAC[i] += _THIS->delta_yAC_TMP[i];
5644 
5645  if ( _THIS->options.numRefinementSteps > 0 )
5646  {
5647  /* compute residuals in tempA and tempB, and max-norm */
5648  for ( i=0; i<nFR; ++i )
5649  {
5650  ii = FR_idx[i];
5651  _THIS->tempA[i] = delta_g[ii];
5652  }
5653 
5654  switch ( _THIS->hessianType )
5655  {
5656  case HST_ZERO:
5657  if ( QProblem_usingRegularisation( _THIS ) == BT_TRUE )
5658  for ( i=0; i<nFR; ++i )
5659  _THIS->tempA[i] += _THIS->regVal*delta_xFR[i];
5660  break;
5661 
5662  case HST_IDENTITY:
5663  for ( i=0; i<nFR; ++i )
5664  _THIS->tempA[i] += delta_xFR[i];
5665  break;
5666 
5667  default:
5668  DenseMatrix_subTimes(_THIS->H,Bounds_getFree( &(_THIS->bounds) ), Bounds_getFree( &(_THIS->bounds) ), 1, 1.0, delta_xFR, nFR, 1.0, _THIS->tempA, nFR, BT_TRUE);
5669  DenseMatrix_subTimes(_THIS->H,Bounds_getFree( &(_THIS->bounds) ), Bounds_getFixed( &(_THIS->bounds) ), 1, 1.0, delta_xFX, nFX, 1.0, _THIS->tempA, nFR, BT_TRUE);
5670  break;
5671  }
5672 
5673  DenseMatrix_subTransTimes(_THIS->A,Constraints_getActive( &(_THIS->constraints)), Bounds_getFree( &(_THIS->bounds) ), 1, -1.0, delta_yAC, nAC, 1.0, _THIS->tempA, nFR);
5674  rnrm = 0.0;
5675  for ( i=0; i<nFR; ++i )
5676  if (rnrm < qpOASES_getAbs (_THIS->tempA[i]))
5677  rnrm = qpOASES_getAbs (_THIS->tempA[i]);
5678 
5679  if (!Delta_bC_isZero)
5680  {
5681  for ( i=0; i<nAC; ++i )
5682  {
5683  ii = AC_idx[i];
5684  if ( Constraints_getStatus( &(_THIS->constraints),ii ) == ST_LOWER )
5685  _THIS->tempB[i] = delta_lbA[ii];
5686  else
5687  _THIS->tempB[i] = delta_ubA[ii];
5688  }
5689  }
5690  else
5691  {
5692  for ( i=0; i<nAC; ++i )
5693  _THIS->tempB[i] = 0.0;
5694  }
5695  DenseMatrix_subTimes(_THIS->A,Constraints_getActive( &(_THIS->constraints)), Bounds_getFree( &(_THIS->bounds) ), 1, -1.0, delta_xFR, nFR, 1.0, _THIS->tempB, nAC, BT_TRUE);
5696  DenseMatrix_subTimes(_THIS->A,Constraints_getActive( &(_THIS->constraints)), Bounds_getFixed( &(_THIS->bounds) ), 1, -1.0, delta_xFX, nFX, 1.0, _THIS->tempB, nAC, BT_TRUE);
5697  for ( i=0; i<nAC; ++i )
5698  if (rnrm < qpOASES_getAbs (_THIS->tempB[i]))
5699  rnrm = qpOASES_getAbs (_THIS->tempB[i]);
5700 
5701  /* early termination of residual norm small enough */
5702  if ( rnrm < _THIS->options.epsIterRef )
5703  break;
5704  }
5705  } /* end of refinement loop for delta_xFRz, delta_xFRy, delta_yAC */
5706 
5707 
5708  /* IV) DETERMINE delta_yFX */
5709  if ( nFX > 0 )
5710  {
5711  for( i=0; i<nFX; ++i )
5712  delta_yFX[i] = delta_g[FX_idx[i]];
5713 
5714  DenseMatrix_subTransTimes(_THIS->A,Constraints_getActive( &(_THIS->constraints)), Bounds_getFixed( &(_THIS->bounds) ), 1, -1.0, delta_yAC, nAC, 1.0, delta_yFX, nFX);
5715 
5716  if ( _THIS->hessianType == HST_ZERO )
5717  {
5718  if ( QProblem_usingRegularisation( _THIS ) == BT_TRUE )
5719  for( i=0; i<nFX; ++i )
5720  delta_yFX[i] += _THIS->regVal*delta_xFX[i];
5721  }
5722  else if ( _THIS->hessianType == HST_IDENTITY )
5723  {
5724  for( i=0; i<nFX; ++i )
5725  delta_yFX[i] += delta_xFX[i];
5726  }
5727  else
5728  {
5729  DenseMatrix_subTimes(_THIS->H,Bounds_getFixed( &(_THIS->bounds) ), Bounds_getFree( &(_THIS->bounds) ), 1, 1.0, delta_xFR, nFR, 1.0, delta_yFX, nFX, BT_TRUE);
5730  DenseMatrix_subTimes(_THIS->H,Bounds_getFixed( &(_THIS->bounds) ), Bounds_getFixed( &(_THIS->bounds) ), 1, 1.0, delta_xFX, nFX, 1.0, delta_yFX, nFX, BT_TRUE);
5731  }
5732  }
5733 
5734  return SUCCESSFUL_RETURN;
5735 }
5736 
5737 
5738 /*
5739  * p e r f o r m S t e p
5740  */
5741 returnValue QProblem_performStep( QProblem* _THIS, const real_t* const delta_g,
5742  const real_t* const delta_lbA, const real_t* const delta_ubA,
5743  const real_t* const delta_lb, const real_t* const delta_ub,
5744  const real_t* const delta_xFX, const real_t* const delta_xFR,
5745  const real_t* const delta_yAC, const real_t* const delta_yFX,
5746  int* BC_idx, SubjectToStatus* BC_status, BooleanType* BC_isBound
5747  )
5748 {
5749  int i, j, ii, jj;
5750  #ifndef __SUPPRESSANYOUTPUT__
5751  myStatic char messageString[QPOASES_MAX_STRING_LENGTH];
5752  #endif
5753 
5754  int nV = QProblem_getNV( _THIS );
5755  int nC = QProblem_getNC( _THIS );
5756  int nFR = QProblem_getNFR( _THIS );
5757  int nFX = QProblem_getNFX( _THIS );
5758  int nAC = QProblem_getNAC( _THIS );
5759  int nIAC = QProblem_getNIAC( _THIS );
5760 
5761  int* FR_idx;
5762  int* FX_idx;
5763  int* AC_idx;
5764  int* IAC_idx;
5765 
5766  int BC_idx_tmp = -1;
5767 
5768  myStatic real_t num[NVCMAX];
5769  myStatic real_t den[NVCMAX];
5770 
5771  myStatic real_t delta_Ax_l[NCMAX];
5772  myStatic real_t delta_Ax_u[NCMAX];
5773  myStatic real_t delta_Ax[NCMAX];
5774 
5775  myStatic real_t delta_x[NVMAX];
5776 
5777  /* initialise maximum steplength array */
5778  _THIS->tau = 1.0;
5779  *BC_idx = -1;
5780  *BC_status = ST_UNDEFINED;
5781  *BC_isBound = BT_FALSE;
5782 
5783 
5784  Indexlist_getNumberArray( Bounds_getFree( &(_THIS->bounds) ),&FR_idx );
5785  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
5786  Indexlist_getNumberArray( Constraints_getActive( &(_THIS->constraints) ),&AC_idx );
5787  Indexlist_getNumberArray( Constraints_getInactive( &(_THIS->constraints) ),&IAC_idx );
5788 
5789  for( j=0; j<nFR; ++j )
5790  {
5791  jj = FR_idx[j];
5792  delta_x[jj] = delta_xFR[j];
5793  }
5794  for( j=0; j<nFX; ++j )
5795  {
5796  jj = FX_idx[j];
5797  delta_x[jj] = delta_xFX[j];
5798  }
5799 
5800 
5801  /* I) DETERMINE MAXIMUM DUAL STEPLENGTH: */
5802  /* 1) Ensure that active dual constraints' bounds remain valid
5803  * (ignoring inequality constraints). */
5804  for( i=0; i<nAC; ++i )
5805  {
5806  ii = AC_idx[i];
5807 
5808  num[i] = _THIS->y[nV+ii];
5809  den[i] = -delta_yAC[i];
5810  }
5811 
5812  QProblem_performRatioTestC( _THIS,nAC,AC_idx,&(_THIS->constraints), num,den, _THIS->options.epsNum,_THIS->options.epsDen, &(_THIS->tau),&BC_idx_tmp );
5813 
5814  if ( BC_idx_tmp >= 0 )
5815  {
5816  *BC_idx = BC_idx_tmp;
5817  *BC_status = ST_INACTIVE;
5818  *BC_isBound = BT_FALSE;
5819  }
5820 
5821 
5822  /* 2) Ensure that active dual bounds remain valid
5823  * (ignoring implicitly fixed variables). */
5824  for( i=0; i<nFX; ++i )
5825  {
5826  ii = FX_idx[i];
5827  num[i] = _THIS->y[ii];
5828  den[i] = -delta_yFX[i];
5829  }
5830 
5831  QProblem_performRatioTestB( _THIS,nFX,FX_idx,&(_THIS->bounds),num,den, _THIS->options.epsNum,_THIS->options.epsDen, &(_THIS->tau),&BC_idx_tmp );
5832 
5833  if ( BC_idx_tmp >= 0 )
5834  {
5835  *BC_idx = BC_idx_tmp;
5836  *BC_status = ST_INACTIVE;
5837  *BC_isBound = BT_TRUE;
5838  }
5839 
5840 
5841  /* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH */
5842  /* 1) Ensure that inactive constraints' bounds remain valid
5843  * (ignoring unbounded constraints). */
5844  /* calculate product A*x */
5845  if ( _THIS->constraintProduct == 0 )
5846  {
5847  DenseMatrix_subTimes(_THIS->A,Constraints_getInactive(&(_THIS->constraints)), 0, 1, 1.0, delta_x, nV, 0.0, delta_Ax, nC, BT_FALSE);
5848  }
5849  else
5850  {
5851  for( i=0; i<nIAC; ++i )
5852  {
5853  ii = IAC_idx[i];
5854 
5855  if ( Constraints_getType( &(_THIS->constraints),ii ) != ST_UNBOUNDED )
5856  {
5857  if ( (*(_THIS->constraintProduct))( ii,delta_x, &(delta_Ax[ii]) ) != 0 )
5858  return THROWERROR( RET_ERROR_IN_CONSTRAINTPRODUCT );
5859  }
5860  }
5861  }
5862 
5863  if ( Constraints_hasNoLower( &(_THIS->constraints) ) == BT_FALSE )
5864  {
5865  for( i=0; i<nIAC; ++i )
5866  {
5867  ii = IAC_idx[i];
5868  num[i] = qpOASES_getMax( _THIS->Ax_l[ii],0.0 );
5869  den[i] = delta_lbA[ii] - delta_Ax[ii];
5870  }
5871 
5872  QProblem_performRatioTestC( _THIS,nIAC,IAC_idx,&(_THIS->constraints), num,den, _THIS->options.epsNum,_THIS->options.epsDen, &(_THIS->tau),&BC_idx_tmp );
5873 
5874  if ( BC_idx_tmp >= 0 )
5875  {
5876  *BC_idx = BC_idx_tmp;
5877  *BC_status = ST_LOWER;
5878  *BC_isBound = BT_FALSE;
5879  }
5880  }
5881 
5882  if ( Constraints_hasNoUpper( &(_THIS->constraints) ) == BT_FALSE )
5883  {
5884  for( i=0; i<nIAC; ++i )
5885  {
5886  ii = IAC_idx[i];
5887  num[i] = qpOASES_getMax( _THIS->Ax_u[ii],0.0 );
5888  den[i] = delta_Ax[ii] - delta_ubA[ii];
5889  }
5890 
5891  QProblem_performRatioTestC( _THIS,nIAC,IAC_idx,&(_THIS->constraints), num,den, _THIS->options.epsNum,_THIS->options.epsDen, &(_THIS->tau),&BC_idx_tmp );
5892 
5893  if ( BC_idx_tmp >= 0 )
5894  {
5895  *BC_idx = BC_idx_tmp;
5896  *BC_status = ST_UPPER;
5897  *BC_isBound = BT_FALSE;
5898  }
5899  }
5900 
5901 
5902  for( i=0; i<nIAC; ++i )
5903  {
5904  ii = IAC_idx[i];
5905 
5906  if ( Constraints_getType( &(_THIS->constraints),ii ) != ST_UNBOUNDED )
5907  {
5908  delta_Ax_l[ii] = delta_Ax[ii] - delta_lbA[ii];
5909  delta_Ax_u[ii] = delta_ubA[ii] - delta_Ax[ii];
5910  }
5911  }
5912 
5913 
5914  /* 2) Ensure that inactive bounds remain valid
5915  * (ignoring unbounded variables). */
5916  /* inactive lower bounds */
5917  if ( Bounds_hasNoLower( &(_THIS->bounds) ) == BT_FALSE )
5918  {
5919  for( i=0; i<nFR; ++i )
5920  {
5921  ii = FR_idx[i];
5922  num[i] = qpOASES_getMax( _THIS->x[ii] - _THIS->lb[ii],0.0 );
5923  den[i] = delta_lb[ii] - delta_xFR[i];
5924  }
5925 
5926  QProblem_performRatioTestB( _THIS,nFR,FR_idx,&(_THIS->bounds),num,den, _THIS->options.epsNum,_THIS->options.epsDen, &(_THIS->tau),&BC_idx_tmp );
5927 
5928  if ( BC_idx_tmp >= 0 )
5929  {
5930  *BC_idx = BC_idx_tmp;
5931  *BC_status = ST_LOWER;
5932  *BC_isBound = BT_TRUE;
5933  }
5934  }
5935 
5936  /* inactive upper bounds */
5937  if ( Bounds_hasNoUpper( &(_THIS->bounds) ) == BT_FALSE )
5938  {
5939  for( i=0; i<nFR; ++i )
5940  {
5941  ii = FR_idx[i];
5942  num[i] = qpOASES_getMax( _THIS->ub[ii] - _THIS->x[ii],0.0 );
5943  den[i] = delta_xFR[i] - delta_ub[ii];
5944  }
5945 
5946  QProblem_performRatioTestB( _THIS,nFR,FR_idx,&(_THIS->bounds),num,den, _THIS->options.epsNum,_THIS->options.epsDen, &(_THIS->tau),&BC_idx_tmp );
5947 
5948  if ( BC_idx_tmp >= 0 )
5949  {
5950  *BC_idx = BC_idx_tmp;
5951  *BC_status = ST_UPPER;
5952  *BC_isBound = BT_TRUE;
5953  }
5954  }
5955 
5956 
5957  #ifndef __SUPPRESSANYOUTPUT__
5958  if ( *BC_status == ST_UNDEFINED )
5959  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"Stepsize is %.15e!",_THIS->tau );
5960  else
5961  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"Stepsize is %.15e! (idx = %d, isBound = %d, status = %d)",_THIS->tau,*BC_idx,*BC_isBound,*BC_status );
5962 
5963  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_STEPSIZE_NONPOSITIVE,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
5964  #endif
5965 
5966 
5967  /* III) PERFORM STEP ALONG HOMOTOPY PATH */
5968  if ( _THIS->tau > QPOASES_ZERO )
5969  {
5970  /* 1) Perform step in primal und dual space... */
5971  for( i=0; i<nFR; ++i )
5972  {
5973  ii = FR_idx[i];
5974  _THIS->x[ii] += _THIS->tau * delta_xFR[i];
5975  }
5976 
5977  for( i=0; i<nFX; ++i )
5978  {
5979  ii = FX_idx[i];
5980  _THIS->x[ii] += _THIS->tau * delta_xFX[i];
5981  _THIS->y[ii] += _THIS->tau * delta_yFX[i];
5982  }
5983 
5984  for( i=0; i<nAC; ++i )
5985  {
5986  ii = AC_idx[i];
5987  _THIS->y[nV+ii] += _THIS->tau * delta_yAC[i];
5988  }
5989 
5990  /* 2) Shift QP data. */
5991  for( i=0; i<nV; ++i )
5992  {
5993  _THIS->g[i] += _THIS->tau * delta_g[i];
5994  _THIS->lb[i] += _THIS->tau * delta_lb[i];
5995  _THIS->ub[i] += _THIS->tau * delta_ub[i];
5996  }
5997 
5998  for( i=0; i<nC; ++i )
5999  {
6000  _THIS->lbA[i] += _THIS->tau * delta_lbA[i];
6001  _THIS->ubA[i] += _THIS->tau * delta_ubA[i];
6002  }
6003 
6004  /* 3) Recompute Ax. */
6005  if ( _THIS->constraintProduct == 0 )
6006  {
6007  DenseMatrix_subTimes( _THIS->A,Constraints_getActive( &(_THIS->constraints)),0, 1, 1.0, _THIS->x, nV, 0.0, _THIS->Ax, nC, BT_FALSE );
6008  }
6009  else
6010  {
6011  for( i=0; i<nAC; ++i )
6012  {
6013  ii = AC_idx[i];
6014 
6015  if ( (*(_THIS->constraintProduct))( ii,_THIS->x, &(_THIS->Ax[ii]) ) != 0 )
6016  return THROWERROR( RET_ERROR_IN_CONSTRAINTPRODUCT );
6017  }
6018  }
6019 
6020  for( i=0; i<nAC; ++i )
6021  {
6022  ii = AC_idx[i];
6023  _THIS->Ax_u[ii] = _THIS->ubA[ii] - _THIS->Ax[ii];
6024  _THIS->Ax_l[ii] = _THIS->Ax[ii] - _THIS->lbA[ii];
6025  }
6026  for( i=0; i<nIAC; ++i )
6027  {
6028  ii = IAC_idx[i];
6029  if ( Constraints_getType( &(_THIS->constraints),ii ) != ST_UNBOUNDED )
6030  {
6031  _THIS->Ax[ii] += _THIS->tau * delta_Ax[ii];
6032  _THIS->Ax_l[ii] += _THIS->tau * delta_Ax_l[ii];
6033  _THIS->Ax_u[ii] += _THIS->tau * delta_Ax_u[ii];
6034  }
6035  }
6036  }
6037  else
6038  {
6039  /* print a stepsize warning if stepsize is zero */
6040  #ifndef __SUPPRESSANYOUTPUT__
6041  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"Stepsize is %.15e",_THIS->tau );
6042  MessageHandling_throwWarning( qpOASES_getGlobalMessageHandler(),RET_STEPSIZE,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
6043  #endif
6044  }
6045 
6046 
6047 
6048  return SUCCESSFUL_RETURN;
6049 }
6050 
6051 
6052 /*
6053  * c h a n g e A c t i v e S e t
6054  */
6055 returnValue QProblem_changeActiveSet( QProblem* _THIS,
6056  int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound
6057  )
6058 {
6059  int nV = QProblem_getNV( _THIS );
6060  myStatic char messageString[QPOASES_MAX_STRING_LENGTH];
6061  returnValue returnvalue;
6062 
6063  switch ( BC_status )
6064  {
6065  /* Optimal solution found as no working set change detected. */
6066  case ST_UNDEFINED:
6067  return SUCCESSFUL_RETURN;
6068 
6069  /* Remove one variable from active set. */
6070  case ST_INACTIVE:
6071  if ( BC_isBound == BT_TRUE )
6072  {
6073  #ifndef __SUPPRESSANYOUTPUT__
6074  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"bound no. %d.", BC_idx );
6075  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_REMOVE_FROM_ACTIVESET,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
6076  #endif
6077 
6078  if ( QProblem_removeBound( _THIS,BC_idx,BT_TRUE,BT_TRUE,_THIS->options.enableNZCTests ) != SUCCESSFUL_RETURN )
6079  return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
6080 
6081  _THIS->y[BC_idx] = 0.0;
6082  }
6083  else
6084  {
6085  #ifndef __SUPPRESSANYOUTPUT__
6086  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"constraint no. %d.", BC_idx );
6087  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_REMOVE_FROM_ACTIVESET,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
6088  #endif
6089 
6090  if ( QProblem_removeConstraint( _THIS,BC_idx,BT_TRUE,BT_TRUE,_THIS->options.enableNZCTests ) != SUCCESSFUL_RETURN )
6091  return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
6092 
6093  _THIS->y[nV+BC_idx] = 0.0;
6094  }
6095  break;
6096 
6097 
6098  /* Add one variable to active set. */
6099  default:
6100  if ( BC_isBound == BT_TRUE )
6101  {
6102  #ifndef __SUPPRESSANYOUTPUT__
6103  if ( BC_status == ST_LOWER )
6104  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"lower bound no. %d.", BC_idx );
6105  else
6106  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"upper bound no. %d.", BC_idx );
6107  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_ADD_TO_ACTIVESET,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
6108  #endif
6109 
6110  returnvalue = QProblem_addBound( _THIS,BC_idx,BC_status,BT_TRUE,BT_TRUE );
6111  if ( returnvalue == RET_ADDBOUND_FAILED_INFEASIBILITY )
6112  return returnvalue;
6113  if ( returnvalue != SUCCESSFUL_RETURN )
6114  return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
6115  }
6116  else
6117  {
6118  #ifndef __SUPPRESSANYOUTPUT__
6119  if ( BC_status == ST_LOWER )
6120  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"lower constraint's bound no. %d.", BC_idx );
6121  else
6122  snprintf( messageString,QPOASES_MAX_STRING_LENGTH,"upper constraint's bound no. %d.", BC_idx );
6123  MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),RET_ADD_TO_ACTIVESET,messageString,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
6124  #endif
6125 
6126  returnvalue = QProblem_addConstraint( _THIS,BC_idx,BC_status,BT_TRUE,BT_TRUE );
6127  if ( returnvalue == RET_ADDCONSTRAINT_FAILED_INFEASIBILITY )
6128  return returnvalue;
6129  if ( returnvalue != SUCCESSFUL_RETURN )
6130  return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
6131  }
6132  }
6133 
6134  return SUCCESSFUL_RETURN;
6135 }
6136 
6137 
6138 
6139 /*
6140  * g e t R e l a t i v e H o m o t o p y L e n g t h
6141  */
6142 real_t QProblem_getRelativeHomotopyLength( QProblem* _THIS,
6143  const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
6144  const real_t* const lbA_new, const real_t* const ubA_new
6145  )
6146 {
6147  int i;
6148  int nC = QProblem_getNC( _THIS );
6149  real_t len = QProblemBCPY_getRelativeHomotopyLength( _THIS, g_new,lb_new,ub_new );
6150  real_t d, s;
6151 
6152  /* fprintf( stdFile, "len in homotopyLength = %.3e\n",len ); */
6153 
6154  /* lower constraint bounds */
6155  if ( lbA_new != 0 )
6156  {
6157  for (i = 0; i < nC; i++)
6158  {
6159  s = qpOASES_getAbs(lbA_new[i]);
6160  if (s < 1.0) s = 1.0;
6161  d = qpOASES_getAbs(lbA_new[i] - _THIS->lbA[i]) / s;
6162  if (d > len) len = d;
6163  }
6164  }
6165 
6166  /* upper constraint bounds */
6167  if ( ubA_new != 0 )
6168  {
6169  for (i = 0; i < nC; i++)
6170  {
6171  s = qpOASES_getAbs(ubA_new[i]);
6172  if (s < 1.0) s = 1.0;
6173  d = qpOASES_getAbs(ubA_new[i] - _THIS->ubA[i]) / s;
6174  if (d > len) len = d;
6175  }
6176  }
6177 
6178  return len;
6179 }
6180 
6181 
6182 /*
6183  * u p d a t e F a r B o u n d s
6184  */
6185 returnValue QProblem_updateFarBounds( QProblem* _THIS,
6186  real_t curFarBound, int nRamp,
6187  const real_t* const lb_new, real_t* const lb_new_far,
6188  const real_t* const ub_new, real_t* const ub_new_far,
6189  const real_t* const lbA_new, real_t* const lbA_new_far,
6190  const real_t* const ubA_new, real_t* const ubA_new_far
6191  )
6192 {
6193  int i;
6194  real_t rampVal, t;
6195  int nV = QProblem_getNV( _THIS );
6196  int nC = QProblem_getNC( _THIS );
6197 
6198  returnValue returnvalue = QProblemBCPY_updateFarBounds( _THIS,curFarBound,nRamp,
6199  lb_new,lb_new_far, ub_new,ub_new_far
6200  );
6201  if ( returnvalue != SUCCESSFUL_RETURN )
6202  return returnvalue;
6203 
6204  if ( _THIS->options.enableRamping == BT_TRUE )
6205  {
6206  for ( i=0; i<nC; ++i )
6207  {
6208  t = (real_t)((nV+i + _THIS->rampOffset) % nRamp) / (real_t)(nRamp-1);
6209  rampVal = curFarBound * (1.0 + (1.0-t)*_THIS->ramp0 + t*_THIS->ramp1);
6210 
6211  if ( lbA_new == 0 )
6212  lbA_new_far[i] = -rampVal;
6213  else
6214  lbA_new_far[i] = qpOASES_getMax( -rampVal,lbA_new[i] );
6215 
6216  if ( ubA_new == 0 )
6217  ubA_new_far[i] = rampVal;
6218  else
6219  ubA_new_far[i] = qpOASES_getMin( rampVal,ubA_new[i] );
6220  }
6221  }
6222  else
6223  {
6224  for ( i=0; i<nC; ++i )
6225  {
6226  if ( lbA_new == 0 )
6227  lbA_new_far[i] = -curFarBound;
6228  else
6229  lbA_new_far[i] = qpOASES_getMax( -curFarBound,lbA_new[i] );
6230 
6231  if ( ubA_new == 0 )
6232  ubA_new_far[i] = curFarBound;
6233  else
6234  ubA_new_far[i] = qpOASES_getMin( curFarBound,ubA_new[i] );
6235  }
6236  }
6237 
6238  return SUCCESSFUL_RETURN;
6239 }
6240 
6241 
6242 /*
6243  * u p d a t e F a r B o u n d s
6244  */
6245 returnValue QProblemBCPY_updateFarBounds( QProblem* _THIS,
6246  real_t curFarBound, int nRamp,
6247  const real_t* const lb_new, real_t* const lb_new_far,
6248  const real_t* const ub_new, real_t* const ub_new_far
6249  )
6250 {
6251  int i;
6252  real_t rampVal, t;
6253  int nV = QProblem_getNV( _THIS );
6254 
6255  if ( _THIS->options.enableRamping == BT_TRUE )
6256  {
6257  for ( i=0; i<nV; ++i )
6258  {
6259  t = (real_t)((i + _THIS->rampOffset) % nRamp) / (real_t)(nRamp-1);
6260  rampVal = curFarBound * (1.0 + (1.0-t)*_THIS->ramp0 + t*_THIS->ramp1);
6261 
6262  if ( lb_new == 0 )
6263  lb_new_far[i] = -rampVal;
6264  else
6265  lb_new_far[i] = qpOASES_getMax( -rampVal,lb_new[i] );
6266 
6267  if ( ub_new == 0 )
6268  ub_new_far[i] = rampVal;
6269  else
6270  ub_new_far[i] = qpOASES_getMin( rampVal,ub_new[i] );
6271  }
6272  }
6273  else
6274  {
6275  for ( i=0; i<nV; ++i )
6276  {
6277  if ( lb_new == 0 )
6278  lb_new_far[i] = -curFarBound;
6279  else
6280  lb_new_far[i] = qpOASES_getMax( -curFarBound,lb_new[i] );
6281 
6282  if ( ub_new == 0 )
6283  ub_new_far[i] = curFarBound;
6284  else
6285  ub_new_far[i] = qpOASES_getMin( curFarBound,ub_new[i] );
6286  }
6287  }
6288 
6289  return SUCCESSFUL_RETURN;
6290 }
6291 
6292 
6293 /*
6294  * p e r f o r m R a m p i n g
6295  */
6296 returnValue QProblem_performRamping( QProblem* _THIS )
6297 {
6298  int nV = QProblem_getNV( _THIS ), nC = QProblem_getNC( _THIS ), bstat, cstat, i, nRamp;
6299  real_t tP, rampValP, tD, rampValD, sca;
6300 
6301  /* compute number of values in ramp */
6302  nRamp = nV + nC + nC + nV;
6303 
6304  /* ramp inactive variable bounds and active dual bound variables */
6305  for (i = 0; i < nV; i++)
6306  {
6307  switch (Bounds_getType(&(_THIS->bounds),i))
6308  {
6309  case ST_EQUALITY:
6310  _THIS->lb[i] = _THIS->x[i]; _THIS->ub[i] = _THIS->x[i]; /* reestablish exact feasibility */
6311  continue;
6312 
6313  case ST_BOUNDED:
6314  tP = (real_t)((i+_THIS->rampOffset) % nRamp) / (real_t)(nRamp-1);
6315  rampValP = (1.0-tP) * _THIS->ramp0 + tP * _THIS->ramp1;
6316  tD = (real_t)((nV+nC+nC+i+_THIS->rampOffset) % nRamp) / (real_t)(nRamp-1);
6317  rampValD = (1.0-tD) * _THIS->ramp0 + tD * _THIS->ramp1;
6318  bstat = Bounds_getStatus( &(_THIS->bounds),i);
6319  if (bstat != ST_LOWER) { sca = qpOASES_getMax(qpOASES_getAbs(_THIS->x[i]), 1.0); _THIS->lb[i] = _THIS->x[i] - sca * rampValP; }
6320  if (bstat != ST_UPPER) { sca = qpOASES_getMax(qpOASES_getAbs(_THIS->x[i]), 1.0); _THIS->ub[i] = _THIS->x[i] + sca * rampValP; }
6321  if (bstat == ST_LOWER) { _THIS->lb[i] = _THIS->x[i]; _THIS->y[i] = +rampValD; }
6322  if (bstat == ST_UPPER) { _THIS->ub[i] = _THIS->x[i]; _THIS->y[i] = -rampValD; }
6323  if (bstat == ST_INACTIVE) _THIS->y[i] = 0.0; /* reestablish exact complementarity */
6324  break;
6325 
6326  case ST_UNBOUNDED:
6327  case ST_DISABLED:
6328  default:
6329  continue;
6330  }
6331  }
6332 
6333  /* ramp inactive constraints and active dual constraint variables */
6334  for (i = 0; i < nC; i++)
6335  {
6336  switch (Constraints_getType( &(_THIS->constraints),i))
6337  {
6338  case ST_EQUALITY:
6339  _THIS->lbA[i] = _THIS->Ax[i]; _THIS->ubA[i] = _THIS->Ax[i]; /* reestablish exact feasibility */
6340  continue;
6341 
6342  case ST_BOUNDED:
6343  tP = (real_t)((nV+i+_THIS->rampOffset) % nRamp) / (real_t)(nRamp-1);
6344  rampValP = (1.0-tP) * _THIS->ramp0 + tP * _THIS->ramp1;
6345  tD = (real_t)((nV+nC+i+_THIS->rampOffset) % nRamp) / (real_t)(nRamp-1);
6346  rampValD = (1.0-tD) * _THIS->ramp0 + tD * _THIS->ramp1;
6347  cstat = Constraints_getStatus( &(_THIS->constraints),i);
6348  if (cstat != ST_LOWER) { sca = qpOASES_getMax(qpOASES_getAbs(_THIS->Ax[i]), 1.0); _THIS->lbA[i] = _THIS->Ax[i] - sca * rampValP; }
6349  if (cstat != ST_UPPER) { sca = qpOASES_getMax(qpOASES_getAbs(_THIS->Ax[i]), 1.0); _THIS->ubA[i] = _THIS->Ax[i] + sca * rampValP; }
6350  if (cstat == ST_LOWER) { _THIS->lbA[i] = _THIS->Ax[i]; _THIS->y[nV+i] = +rampValD; }
6351  if (cstat == ST_UPPER) { _THIS->ubA[i] = _THIS->Ax[i]; _THIS->y[nV+i] = -rampValD; }
6352  if (cstat == ST_INACTIVE) _THIS->y[nV+i] = 0.0; /* reestablish exact complementarity */
6353 
6354  _THIS->Ax_l[i] = _THIS->Ax[i] - _THIS->lbA[i];
6355  _THIS->Ax_u[i] = _THIS->ubA[i] - _THIS->Ax[i];
6356  break;
6357 
6358  case ST_UNBOUNDED:
6359  case ST_DISABLED:
6360  default:
6361  continue;
6362  }
6363  }
6364 
6365  /* reestablish exact stationarity */
6366  QProblem_setupAuxiliaryQPgradient( _THIS );
6367 
6368  /* advance ramp offset to avoid Ramping cycles */
6369  (_THIS->rampOffset)++;
6370 
6371  return SUCCESSFUL_RETURN;
6372 }
6373 
6374 
6375 
6376 /*
6377  * p e r f o r m R a t i o T e s t
6378  */
6379 returnValue QProblem_performRatioTestC( QProblem* _THIS,
6380  int nIdx,
6381  const int* const idxList,
6382  Constraints* const subjectTo,
6383  const real_t* const num,
6384  const real_t* const den,
6385  real_t epsNum,
6386  real_t epsDen,
6387  real_t* t,
6388  int* BC_idx
6389  )
6390 {
6391  int i, ii;
6392 
6393  *BC_idx = -1;
6394 
6395  for( i=0; i<nIdx; ++i )
6396  {
6397  ii = idxList[i];
6398 
6399  if ( Constraints_getType( subjectTo,ii ) != ST_EQUALITY )
6400  {
6401  if ( ( Constraints_getStatus( subjectTo,ii ) == ST_LOWER ) || ( Constraints_getStatus( subjectTo,ii ) == ST_INACTIVE ) )
6402  {
6403  if ( QProblem_isBlocking( _THIS,num[i],den[i],epsNum,epsDen,t ) == BT_TRUE )
6404  {
6405  *t = num[i] / den[i];
6406  *BC_idx = ii;
6407  }
6408  }
6409  else
6410  if ( Constraints_getStatus( subjectTo,ii ) == ST_UPPER )
6411  {
6412  if ( QProblem_isBlocking( _THIS,-num[i],-den[i],epsNum,epsDen,t ) == BT_TRUE )
6413  {
6414  *t = num[i] / den[i];
6415  *BC_idx = ii;
6416  }
6417  }
6418  }
6419  }
6420 
6421  return SUCCESSFUL_RETURN;
6422 }
6423 
6424 
6425 
6426 /*
6427  * p e r f o r m D r i f t C o r r e c t i o n
6428  */
6429 returnValue QProblem_performDriftCorrection( QProblem* _THIS )
6430 {
6431  int i;
6432  int nV = QProblem_getNV( _THIS );
6433  int nC = QProblem_getNC( _THIS );
6434 
6435  for ( i=0; i<nV; ++i )
6436  {
6437  switch ( Bounds_getType( &(_THIS->bounds),i ) )
6438  {
6439  case ST_BOUNDED:
6440  switch ( Bounds_getStatus( &(_THIS->bounds),i ) )
6441  {
6442  case ST_LOWER:
6443  _THIS->lb[i] = _THIS->x[i];
6444  _THIS->ub[i] = qpOASES_getMax (_THIS->ub[i], _THIS->x[i]);
6445  _THIS->y[i] = qpOASES_getMax (_THIS->y[i], 0.0);
6446  break;
6447  case ST_UPPER:
6448  _THIS->lb[i] = qpOASES_getMin (_THIS->lb[i], _THIS->x[i]);
6449  _THIS->ub[i] = _THIS->x[i];
6450  _THIS->y[i] = qpOASES_getMin (_THIS->y[i], 0.0);
6451  break;
6452  case ST_INACTIVE:
6453  _THIS->lb[i] = qpOASES_getMin (_THIS->lb[i], _THIS->x[i]);
6454  _THIS->ub[i] = qpOASES_getMax (_THIS->ub[i], _THIS->x[i]);
6455  _THIS->y[i] = 0.0;
6456  break;
6457  case ST_UNDEFINED:
6458  case ST_INFEASIBLE_LOWER:
6459  case ST_INFEASIBLE_UPPER:
6460  break;
6461  }
6462  break;
6463  case ST_EQUALITY:
6464  _THIS->lb[i] = _THIS->x[i];
6465  _THIS->ub[i] = _THIS->x[i];
6466  break;
6467  case ST_UNBOUNDED:
6468  case ST_UNKNOWN:
6469  case ST_DISABLED:
6470  break;
6471  }
6472  }
6473 
6474  for ( i=0; i<nC; ++i )
6475  {
6476  switch ( Constraints_getType( &(_THIS->constraints),i ) )
6477  {
6478  case ST_BOUNDED:
6479  switch ( Constraints_getStatus( &(_THIS->constraints),i ) )
6480  {
6481  case ST_LOWER:
6482  _THIS->lbA[i] = _THIS->Ax[i];
6483  _THIS->Ax_l[i] = 0.0;
6484  _THIS->ubA[i] = qpOASES_getMax (_THIS->ubA[i], _THIS->Ax[i]);
6485  _THIS->Ax_u[i] = _THIS->ubA[i] - _THIS->Ax[i];
6486  _THIS->y[i+nV] = qpOASES_getMax (_THIS->y[i+nV], 0.0);
6487  break;
6488  case ST_UPPER:
6489  _THIS->lbA[i] = qpOASES_getMin (_THIS->lbA[i], _THIS->Ax[i]);
6490  _THIS->Ax_l[i] = _THIS->Ax[i] - _THIS->lbA[i];
6491  _THIS->ubA[i] = _THIS->Ax[i];
6492  _THIS->Ax_u[i] = 0.0;
6493  _THIS->y[i+nV] = qpOASES_getMin (_THIS->y[i+nV], 0.0);
6494  break;
6495  case ST_INACTIVE:
6496  _THIS->lbA[i] = qpOASES_getMin (_THIS->lbA[i], _THIS->Ax[i]);
6497  _THIS->Ax_l[i] = _THIS->Ax[i] - _THIS->lbA[i];
6498  _THIS->ubA[i] = qpOASES_getMax (_THIS->ubA[i], _THIS->Ax[i]);
6499  _THIS->Ax_u[i] = _THIS->ubA[i] - _THIS->Ax[i];
6500  _THIS->y[i+nV] = 0.0;
6501  break;
6502  case ST_UNDEFINED:
6503  case ST_INFEASIBLE_LOWER:
6504  case ST_INFEASIBLE_UPPER:
6505  break;
6506  }
6507  break;
6508  case ST_EQUALITY:
6509  _THIS->lbA[i] = _THIS->Ax[i];
6510  _THIS->Ax_l[i] = 0.0;
6511  _THIS->ubA[i] = _THIS->Ax[i];
6512  _THIS->Ax_u[i] = 0.0;
6513  break;
6514  case ST_UNBOUNDED:
6515  case ST_UNKNOWN:
6516  case ST_DISABLED:
6517  break;
6518  }
6519  }
6520 
6521  return QProblem_setupAuxiliaryQPgradient( _THIS );
6522 }
6523 
6524 
6525 /*
6526  * s e t u p A u x i l i a r y Q P
6527  */
6528 returnValue QProblem_setupAuxiliaryQP( QProblem* _THIS, Bounds* const guessedBounds, Constraints* const guessedConstraints )
6529 {
6530  int i, j;
6531  int nV = QProblem_getNV( _THIS );
6532  int nC = QProblem_getNC( _THIS );
6533 
6534  /* consistency check */
6535  if ( ( guessedBounds == 0 ) || ( guessedConstraints == 0 ) )
6536  return THROWERROR( RET_INVALID_ARGUMENTS );
6537 
6538  /* nothing to do */
6539  if ( ( guessedBounds == &(_THIS->bounds) ) && ( guessedConstraints == &(_THIS->constraints) ) )
6540  return SUCCESSFUL_RETURN;
6541 
6542  _THIS->status = QPS_PREPARINGAUXILIARYQP;
6543 
6544 
6545  /* I) SETUP WORKING SET ... */
6546  if ( QProblem_shallRefactorise( _THIS,guessedBounds,guessedConstraints ) == BT_TRUE )
6547  {
6548  /* ... WITH REFACTORISATION: */
6549  /* 1) Reset bounds/constraints ... */
6550  Bounds_init( &(_THIS->bounds),nV );
6551  Constraints_init( &(_THIS->constraints),nC );
6552 
6553  /* ... and set them up afresh. */
6554  if ( QProblem_setupSubjectToType( _THIS ) != SUCCESSFUL_RETURN )
6555  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
6556 
6557  if ( Bounds_setupAllFree( &(_THIS->bounds) ) != SUCCESSFUL_RETURN )
6558  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
6559 
6560  if ( Constraints_setupAllInactive( &(_THIS->constraints) ) != SUCCESSFUL_RETURN )
6561  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
6562 
6563  /* 2) Setup TQ factorisation. */
6564  if ( QProblem_setupTQfactorisation( _THIS ) != SUCCESSFUL_RETURN )
6565  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
6566 
6567  /* 3) Setup guessed working sets afresh. */
6568  if ( QProblem_setupAuxiliaryWorkingSet( _THIS,guessedBounds,guessedConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
6569  THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
6570 
6571  /* 4) Computes Cholesky decomposition of projected Hessian
6572  * This now handles all special cases (no active bounds/constraints, no nullspace) */
6573  if ( QProblem_computeProjectedCholesky( _THIS ) != SUCCESSFUL_RETURN )
6574  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
6575  }
6576  else
6577  {
6578  /* ... WITHOUT REFACTORISATION: */
6579  if ( QProblem_setupAuxiliaryWorkingSet( _THIS,guessedBounds,guessedConstraints,BT_FALSE ) != SUCCESSFUL_RETURN )
6580  THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
6581  }
6582 
6583 
6584  /* II) SETUP AUXILIARY QP DATA: */
6585  /* 1) Ensure that dual variable is zero for free bounds and inactive constraints. */
6586  for ( i=0; i<nV; ++i )
6587  if ( Bounds_getStatus( &(_THIS->bounds),i ) == ST_INACTIVE )
6588  _THIS->y[i] = 0.0;
6589 
6590  for ( i=0; i<nC; ++i )
6591  if ( Constraints_getStatus( &(_THIS->constraints),i ) == ST_INACTIVE )
6592  _THIS->y[nV+i] = 0.0;
6593 
6594  /* 2) Setup gradient and (constraints') bound vectors. */
6595  if ( QProblem_setupAuxiliaryQPgradient( _THIS ) != SUCCESSFUL_RETURN )
6596  THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
6597 
6598  DenseMatrix_times(_THIS->A,1, 1.0, _THIS->x, nV, 0.0, _THIS->Ax, nC);
6599  for ( j=0; j<nC; ++j )
6600  {
6601  _THIS->Ax_l[j] = _THIS->Ax[j];
6602  _THIS->Ax_u[j] = _THIS->Ax[j];
6603  }
6604 
6605  /* (also sets Ax_l and Ax_u) */
6606  if ( QProblem_setupAuxiliaryQPbounds( _THIS,0,0,BT_FALSE ) != SUCCESSFUL_RETURN )
6607  THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
6608 
6609  return SUCCESSFUL_RETURN;
6610 }
6611 
6612 
6613 /*
6614  * s h a l l R e f a c t o r i s e
6615  */
6616 
6617 BooleanType QProblem_shallRefactorise( QProblem* _THIS,
6618  Bounds* const guessedBounds,
6619  Constraints* const guessedConstraints
6620  )
6621 {
6622  int i;
6623  int nV = QProblem_getNV( _THIS );
6624  int nC = QProblem_getNC( _THIS );
6625 
6626  int differenceNumberBounds = 0;
6627  int differenceNumberConstraints = 0;
6628 
6629  /* always refactorise if Hessian is not known to be positive definite */
6630  if ( ( _THIS->hessianType == HST_SEMIDEF ) || ( _THIS->hessianType == HST_INDEF ) )
6631  return BT_TRUE;
6632 
6633  /* 1) Determine number of bounds that have same status
6634  * in guessed AND current bounds.*/
6635  for( i=0; i<nV; ++i )
6636  if ( Bounds_getStatus( guessedBounds,i ) != Bounds_getStatus( &(_THIS->bounds),i ) )
6637  ++differenceNumberBounds;
6638 
6639  /* 2) Determine number of constraints that have same status
6640  * in guessed AND current constraints.*/
6641  for( i=0; i<nC; ++i )
6642  if ( Constraints_getStatus( guessedConstraints,i ) != Constraints_getStatus( &(_THIS->constraints),i ) )
6643  ++differenceNumberConstraints;
6644 
6645  /* 3) Decide wheter to refactorise or not. */
6646  if ( 2*(differenceNumberBounds+differenceNumberConstraints) > Constraints_getNAC( guessedConstraints )+Bounds_getNFX( guessedBounds ) )
6647  return BT_TRUE;
6648  else
6649  return BT_FALSE;
6650 }
6651 
6652 
6653 /*
6654  * s e t u p Q P d a t a
6655  */
6656 returnValue QProblem_setupQPdataM( QProblem* _THIS,
6657  DenseMatrix *_H, const real_t* const _g, DenseMatrix *_A,
6658  const real_t* const _lb, const real_t* const _ub,
6659  const real_t* const _lbA, const real_t* const _ubA
6660  )
6661 {
6662  if ( _H == 0 )
6663  {
6664  if ( _A == 0 )
6665  return QProblem_setupQPdata( _THIS,(real_t*)0,_g,(real_t*)0,_lb,_ub,_lbA,_ubA );
6666  else
6667  return QProblem_setupQPdata( _THIS,(real_t*)0,_g,DenseMatrix_getVal(_A),_lb,_ub,_lbA,_ubA );
6668  }
6669  else
6670  {
6671  if ( _A == 0 )
6672  return QProblem_setupQPdata( _THIS,DenseMatrix_getVal(_H),_g,(real_t*)0,_lb,_ub,_lbA,_ubA );
6673  else
6674  return QProblem_setupQPdata( _THIS,DenseMatrix_getVal(_H),_g,DenseMatrix_getVal(_A),_lb,_ub,_lbA,_ubA );
6675  }
6676 }
6677 
6678 
6679 /*
6680  * s e t u p Q P d a t a
6681  */
6682 returnValue QProblem_setupQPdata( QProblem* _THIS,
6683  real_t* const _H, const real_t* const _g, real_t* const _A,
6684  const real_t* const _lb, const real_t* const _ub,
6685  const real_t* const _lbA, const real_t* const _ubA
6686  )
6687 {
6688  int nC = QProblem_getNC( _THIS );
6689 
6690  /* 1) Load Hessian matrix as well as lower and upper bounds vectors. */
6691  if ( QProblemBCPY_setupQPdata( _THIS,_H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
6692  return THROWERROR( RET_INVALID_ARGUMENTS );
6693 
6694  if ( ( nC > 0 ) && ( _A == 0 ) )
6695  return THROWERROR( RET_INVALID_ARGUMENTS );
6696 
6697  if ( nC > 0 )
6698  {
6699  /* 2) Setup lower/upper constraints' bounds vector. */
6700  QProblem_setLBA( _THIS,_lbA );
6701  QProblem_setUBA( _THIS,_ubA );
6702 
6703  /* 3) Only load constraint matrix after setting up vectors! */
6704  QProblem_setA( _THIS,_A );
6705  }
6706  else
6707  {
6708  DenseMatrixCON( _THIS->A, 0,0,0, 0 );
6709  }
6710 
6711  return SUCCESSFUL_RETURN;
6712 }
6713 
6714 
6715 /*
6716  * s e t u p Q P d a t a F r o m F i l e
6717  */
6718 returnValue QProblem_setupQPdataFromFile( QProblem* _THIS,
6719  const char* const H_file, const char* const g_file, const char* const A_file,
6720  const char* const lb_file, const char* const ub_file,
6721  const char* const lbA_file, const char* const ubA_file
6722  )
6723 {
6724  int i;
6725  int nV = QProblem_getNV( _THIS );
6726  int nC = QProblem_getNC( _THIS );
6727 
6728  returnValue returnvalue;
6729 
6730  myStatic real_t _A[NCMAX*NVMAX];
6731 
6732  /* 1) Load Hessian matrix as well as lower and upper bounds vectors from files. */
6733  returnvalue = QProblemBCPY_setupQPdataFromFile( _THIS,H_file,g_file,lb_file,ub_file );
6734  if ( returnvalue != SUCCESSFUL_RETURN )
6735  return THROWERROR( returnvalue );
6736 
6737  if ( ( nC > 0 ) && ( A_file == 0 ) )
6738  return THROWERROR( RET_INVALID_ARGUMENTS );
6739 
6740  if ( nC > 0 )
6741  {
6742  /* 2) Load lower constraints' bounds vector from file. */
6743  if ( lbA_file != 0 )
6744  {
6745  returnvalue = qpOASES_readFromFileV( _THIS->lbA, nC, lbA_file );
6746  if ( returnvalue != SUCCESSFUL_RETURN )
6747  return THROWERROR( returnvalue );
6748  }
6749  else
6750  {
6751  /* if no lower constraints' bounds are specified, set them to -infinity */
6752  for( i=0; i<nC; ++i )
6753  _THIS->lbA[i] = -QPOASES_INFTY;
6754  }
6755 
6756  /* 3) Load upper constraints' bounds vector from file. */
6757  if ( ubA_file != 0 )
6758  {
6759  returnvalue = qpOASES_readFromFileV( _THIS->ubA, nC, ubA_file );
6760  if ( returnvalue != SUCCESSFUL_RETURN )
6761  return THROWERROR( returnvalue );
6762  }
6763  else
6764  {
6765  /* if no upper constraints' bounds are specified, set them to infinity */
6766  for( i=0; i<nC; ++i )
6767  _THIS->ubA[i] = QPOASES_INFTY;
6768  }
6769 
6770  /* 4) Only load constraint matrix from file after setting up vectors! */
6771  returnvalue = qpOASES_readFromFileM( _A, nC,nV, A_file );
6772  if ( returnvalue != SUCCESSFUL_RETURN )
6773  return THROWERROR( returnvalue );
6774  QProblem_setA( _THIS,_A );
6775 
6776  }
6777 
6778  return SUCCESSFUL_RETURN;
6779 }
6780 
6781 
6782 /*
6783  * l o a d Q P v e c t o r s F r o m F i l e
6784  */
6785 returnValue QProblem_loadQPvectorsFromFile( QProblem* _THIS,
6786  const char* const g_file, const char* const lb_file, const char* const ub_file,
6787  const char* const lbA_file, const char* const ubA_file,
6788  real_t* const g_new, real_t* const lb_new, real_t* const ub_new,
6789  real_t* const lbA_new, real_t* const ubA_new
6790  )
6791 {
6792  int nC = QProblem_getNC( _THIS );
6793 
6794  returnValue returnvalue;
6795 
6796 
6797  /* 1) Load gradient vector as well as lower and upper bounds vectors from files. */
6798  returnvalue = QProblemBCPY_loadQPvectorsFromFile( _THIS,g_file,lb_file,ub_file, g_new,lb_new,ub_new );
6799  if ( returnvalue != SUCCESSFUL_RETURN )
6800  return THROWERROR( returnvalue );
6801 
6802  if ( nC > 0 )
6803  {
6804  /* 2) Load lower constraints' bounds vector from file. */
6805  if ( lbA_file != 0 )
6806  {
6807  if ( lbA_new != 0 )
6808  {
6809  returnvalue = qpOASES_readFromFileV( lbA_new, nC, lbA_file );
6810  if ( returnvalue != SUCCESSFUL_RETURN )
6811  return THROWERROR( returnvalue );
6812  }
6813  else
6814  {
6815  /* If filename is given, storage must be provided! */
6816  return THROWERROR( RET_INVALID_ARGUMENTS );
6817  }
6818  }
6819 
6820  /* 3) Load upper constraints' bounds vector from file. */
6821  if ( ubA_file != 0 )
6822  {
6823  if ( ubA_new != 0 )
6824  {
6825  returnvalue = qpOASES_readFromFileV( ubA_new, nC, ubA_file );
6826  if ( returnvalue != SUCCESSFUL_RETURN )
6827  return THROWERROR( returnvalue );
6828  }
6829  else
6830  {
6831  /* If filename is given, storage must be provided! */
6832  return THROWERROR( RET_INVALID_ARGUMENTS );
6833  }
6834  }
6835  }
6836 
6837  return SUCCESSFUL_RETURN;
6838 }
6839 
6840 
6841 /*
6842  * p r i n t I t e r a t i o n
6843  */
6844 returnValue QProblem_printIteration( QProblem* _THIS,
6845  int iter,
6846  int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound,
6847  real_t homotopyLength,
6848  BooleanType isFirstCall
6849  )
6850 {
6851  #ifndef __SUPPRESSANYOUTPUT__
6852 
6853  int i;
6854  int nV = QProblem_getNV( _THIS );
6855  int nC = QProblem_getNC( _THIS );
6856  int nAC = QProblem_getNAC( _THIS );
6857 
6858  real_t stat, bfeas, cfeas, bcmpl, ccmpl, Tmaxomin;
6859  real_t Tmin, Tmax;
6860 
6861  myStatic real_t grad[NVMAX];
6862  myStatic real_t AX[NCMAX];
6863 
6864  myStatic char myPrintfString[QPOASES_MAX_STRING_LENGTH];
6865  myStatic char info[QPOASES_MAX_STRING_LENGTH];
6866  const char excStr[] = " ef";
6867 
6868  /* consistency check */
6869  if ( iter < 0 )
6870  return THROWERROR( RET_INVALID_ARGUMENTS );
6871 
6872  switch ( _THIS->options.printLevel )
6873  {
6874  case PL_DEBUG_ITER:
6875  stat = bfeas = cfeas = bcmpl = ccmpl = Tmaxomin = 0.0;
6876 
6877  /* stationarity */
6878  for (i = 0; i < nV; i++) grad[i] = _THIS->g[i] - _THIS->y[i];
6879 
6880  switch ( _THIS->hessianType )
6881  {
6882  case HST_ZERO:
6883  for( i=0; i<nV; ++i )
6884  grad[i] += _THIS->regVal * _THIS->x[i];
6885  break;
6886 
6887  case HST_IDENTITY:
6888  for( i=0; i<nV; ++i )
6889  grad[i] += 1.0 * _THIS->x[i];
6890  break;
6891 
6892  default:
6893  DenseMatrix_times(_THIS->H,1, 1.0, _THIS->x, nV, 1.0, grad, nV);
6894  break;
6895  }
6896 
6897  DenseMatrix_transTimes(_THIS->A,1, -1.0, _THIS->y+nV, nC, 1.0, grad, nV);
6898  for (i = 0; i < nV; i++) if (qpOASES_getAbs(grad[i]) > stat) stat = qpOASES_getAbs(grad[i]);
6899 
6900  /* feasibility */
6901  for (i = 0; i < nV; i++) if (_THIS->lb[i] - _THIS->x[i] > bfeas) bfeas = _THIS->lb[i] - _THIS->x[i];
6902  for (i = 0; i < nV; i++) if (_THIS->x[i] - _THIS->ub[i] > bfeas) bfeas = _THIS->x[i] - _THIS->ub[i];
6903  DenseMatrix_times(_THIS->A,1, 1.0, _THIS->x, nV, 0.0, AX, nC);
6904  for (i = 0; i < nC; i++) if (_THIS->lbA[i] - AX[i] > cfeas) cfeas = _THIS->lbA[i] - AX[i];
6905  for (i = 0; i < nC; i++) if (AX[i] - _THIS->ubA[i] > cfeas) cfeas = AX[i] - _THIS->ubA[i];
6906 
6907  /* complementarity */
6908  for (i = 0; i < nV; i++) if (_THIS->y[i] > +QPOASES_EPS && qpOASES_getAbs((_THIS->lb[i] - _THIS->x[i])*_THIS->y[i]) > bcmpl) bcmpl = qpOASES_getAbs((_THIS->lb[i] - _THIS->x[i])*_THIS->y[i]);
6909  for (i = 0; i < nV; i++) if (_THIS->y[i] < -QPOASES_EPS && qpOASES_getAbs((_THIS->ub[i] - _THIS->x[i])*_THIS->y[i]) > bcmpl) bcmpl = qpOASES_getAbs((_THIS->ub[i] - _THIS->x[i])*_THIS->y[i]);
6910  for (i = 0; i < nC; i++) if (_THIS->y[nV+i] > +QPOASES_EPS && qpOASES_getAbs((_THIS->lbA[i]-AX[i])*_THIS->y[nV+i]) > ccmpl) ccmpl = qpOASES_getAbs((_THIS->lbA[i]-AX[i])*_THIS->y[nV+i]);
6911  for (i = 0; i < nC; i++) if (_THIS->y[nV+i] < -QPOASES_EPS && qpOASES_getAbs((_THIS->ubA[i]-AX[i])*_THIS->y[nV+i]) > ccmpl) ccmpl = qpOASES_getAbs((_THIS->ubA[i]-AX[i])*_THIS->y[nV+i]);
6912 
6913  Tmin = 1.0e16; Tmax = 0.0;
6914  for (i = 0; i < nAC; i++)
6915  if (qpOASES_getAbs(TT(i,_THIS->sizeT-i-1)) < Tmin)
6916  Tmin = qpOASES_getAbs(TT(i,_THIS->sizeT-i-1));
6917  else if (qpOASES_getAbs(TT(i,_THIS->sizeT-i-1)) > Tmax)
6918  Tmax = qpOASES_getAbs(TT(i,_THIS->sizeT-i-1));
6919  Tmaxomin = Tmax/Tmin;
6920 
6921  if ( ( iter % 10 == 0 ) && ( isFirstCall == BT_TRUE ) )
6922  {
6923  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "\n%5s %4s %4s %4s %4s %9s %9s %9s %9s %9s %9s %9s %9s\n",
6924  "iter", "addB", "remB", "addC", "remC", "hom len", "tau", "stat",
6925  "bfeas", "cfeas", "bcmpl", "ccmpl", "Tmin");
6926  qpOASES_myPrintf( myPrintfString );
6927  }
6928 
6929  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%5d ", iter);
6930  qpOASES_myPrintf( myPrintfString );
6931 
6932  if (_THIS->tabularOutput.idxAddB >= 0)
6933  {
6934  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%4d ", _THIS->tabularOutput.idxAddB);
6935  qpOASES_myPrintf( myPrintfString );
6936  }
6937  else
6938  {
6939  qpOASES_myPrintf( " " );
6940  }
6941 
6942  if (_THIS->tabularOutput.idxRemB >= 0)
6943  {
6944  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%4d ", _THIS->tabularOutput.idxRemB);
6945  qpOASES_myPrintf( myPrintfString );
6946  }
6947  else
6948  {
6949  qpOASES_myPrintf( " " );
6950  }
6951 
6952  if (_THIS->tabularOutput.idxAddC >= 0)
6953  {
6954  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%4d ", _THIS->tabularOutput.idxAddC);
6955  qpOASES_myPrintf( myPrintfString );
6956  }
6957  else
6958  {
6959  qpOASES_myPrintf( " " );
6960  }
6961 
6962  if (_THIS->tabularOutput.idxRemC >= 0)
6963  {
6964  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%4d ", _THIS->tabularOutput.idxRemC);
6965  qpOASES_myPrintf( myPrintfString );
6966  }
6967  else
6968  {
6969  qpOASES_myPrintf( " " );
6970  }
6971 
6972  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%9.2e %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e\n",
6973  homotopyLength, _THIS->tau, stat, bfeas, cfeas, bcmpl, ccmpl, Tmin);
6974  qpOASES_myPrintf( myPrintfString );
6975  break;
6976 
6977  case PL_TABULAR:
6978  if ( ( iter % 10 == 0 ) && ( isFirstCall == BT_TRUE ) )
6979  {
6980  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "\n%5s %6s %6s %6s %6s %9s %9s\n",
6981  "iter", "addB", "remB", "addC", "remC", "hom len", "tau" );
6982  qpOASES_myPrintf( myPrintfString );
6983  }
6984  if ( isFirstCall == BT_TRUE )
6985  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%5d ",iter);
6986  else
6987  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%5d*",iter);
6988  qpOASES_myPrintf( myPrintfString );
6989 
6990  if (_THIS->tabularOutput.idxAddB >= 0)
6991  {
6992  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%5d%c ", _THIS->tabularOutput.idxAddB, excStr[_THIS->tabularOutput.excAddB]);
6993  qpOASES_myPrintf( myPrintfString );
6994  }
6995  else
6996  {
6997  qpOASES_myPrintf( " " );
6998  }
6999 
7000  if (_THIS->tabularOutput.idxRemB >= 0)
7001  {
7002  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%5d%c ", _THIS->tabularOutput.idxRemB, excStr[_THIS->tabularOutput.excRemB]);
7003  qpOASES_myPrintf( myPrintfString );
7004  }
7005  else
7006  {
7007  qpOASES_myPrintf( " " );
7008  }
7009 
7010  if (_THIS->tabularOutput.idxAddC >= 0)
7011  {
7012  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%5d%c ", _THIS->tabularOutput.idxAddC, excStr[_THIS->tabularOutput.excAddC]);
7013  qpOASES_myPrintf( myPrintfString );
7014  }
7015  else
7016  {
7017  qpOASES_myPrintf( " " );
7018  }
7019 
7020  if (_THIS->tabularOutput.idxRemC >= 0)
7021  {
7022  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%5d%c ", _THIS->tabularOutput.idxRemC, excStr[_THIS->tabularOutput.excRemC]);
7023  qpOASES_myPrintf( myPrintfString );
7024  }
7025  else
7026  {
7027  qpOASES_myPrintf( " " );
7028  }
7029 
7030  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH, "%9.2e %9.2e\n", homotopyLength, _THIS->tau);
7031  qpOASES_myPrintf( myPrintfString );
7032  break;
7033 
7034  case PL_MEDIUM:
7035  /* 1) Print header at first iteration. */
7036  if ( ( iter == 0 ) && ( isFirstCall == BT_TRUE ) )
7037  {
7038  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH,"\n\n#################### qpOASES -- QP NO. %3.0d #####################\n\n", _THIS->count );
7039  qpOASES_myPrintf( myPrintfString );
7040 
7041  qpOASES_myPrintf( " Iter | StepLength | Info | nFX | nAC \n" );
7042  qpOASES_myPrintf( " ----------+------------------+------------------+---------+--------- \n" );
7043  }
7044 
7045  /* 2) Print iteration line. */
7046  if ( BC_status == ST_UNDEFINED )
7047  {
7048  if ( _THIS->hessianType == HST_ZERO )
7049  snprintf( info,3,"LP" );
7050  else
7051  snprintf( info,3,"QP" );
7052 
7053  if ( isFirstCall == BT_TRUE )
7054  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH," %5.1d | %1.6e | %s SOLVED | %4.1d | %4.1d \n", iter,_THIS->tau,info,QProblem_getNFX( _THIS ),QProblem_getNAC( _THIS ) );
7055  else
7056  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH," %5.1d* | %1.6e | %s SOLVED | %4.1d | %4.1d \n", iter,_THIS->tau,info,QProblem_getNFX( _THIS ),QProblem_getNAC( _THIS ) );
7057  qpOASES_myPrintf( myPrintfString );
7058  }
7059  else
7060  {
7061  if ( BC_status == ST_INACTIVE )
7062  snprintf( info,5,"REM " );
7063  else
7064  snprintf( info,5,"ADD " );
7065 
7066  if ( BC_isBound == BT_TRUE )
7067  snprintf( &(info[4]),4,"BND" );
7068  else
7069  snprintf( &(info[4]),4,"CON" );
7070 
7071  snprintf( myPrintfString,QPOASES_MAX_STRING_LENGTH," %5.1d | %1.6e | %s %4.1d | %4.1d | %4.1d \n", iter,_THIS->tau,info,BC_idx,QProblem_getNFX( _THIS ),QProblem_getNAC( _THIS ) );
7072  qpOASES_myPrintf( myPrintfString );
7073  }
7074  break;
7075 
7076  default:
7077  /* nothing to display */
7078  break;
7079  }
7080 
7081  #endif /* __SUPPRESSANYOUTPUT__ */
7082 
7083  return SUCCESSFUL_RETURN;
7084 }
7085 
7086 
7087 /*
7088  * a r e B o u n d s C o n s i s t e n t
7089  */
7090 returnValue QProblem_areBoundsConsistent( QProblem* _THIS,
7091  const real_t* const lb_new, const real_t* const ub_new,
7092  const real_t* const lbA_new, const real_t* const ubA_new
7093  )
7094 {
7095  int i;
7096 
7097  if (lb_new && ub_new) {
7098  for (i = 0; i < QProblem_getNV(_THIS); ++i) {
7099  if (lb_new[i] > ub_new[i]+QPOASES_EPS) {
7100  return RET_QP_INFEASIBLE;
7101  }
7102  }
7103  }
7104 
7105  if (lbA_new && ubA_new) {
7106  for (i = 0; i < QProblem_getNC(_THIS); ++i) {
7107  if (lbA_new[i] > ubA_new[i]+QPOASES_EPS) {
7108  return RET_QP_INFEASIBLE;
7109  }
7110  }
7111  }
7112 
7113  return SUCCESSFUL_RETURN;
7114 }
7115 
7116 
7117 
7118 /*
7119  * d r o p I n f e a s i b l e s
7120  */
7121 returnValue QProblem_dropInfeasibles( QProblem* _THIS,
7122  int BC_number, SubjectToStatus BC_status, BooleanType BC_isBound,
7123  real_t *xiB, real_t *xiC
7124  )
7125 {
7126  int i;
7127 
7128  int nAC = QProblem_getNAC( _THIS );
7129  int nFX = QProblem_getNFX( _THIS );
7130  int blockingPriority = (BC_isBound) ? _THIS->options.dropBoundPriority : _THIS->options.dropIneqConPriority;
7131  int y_min_number = -1;
7132  BooleanType y_min_isBound = BC_isBound;
7133  int y_min_priority = blockingPriority;
7134 
7135  int *AC_idx, *FX_idx;
7136 
7137  Indexlist_getNumberArray( Constraints_getActive( &(_THIS->constraints) ),&AC_idx );
7138  Indexlist_getNumberArray( Bounds_getFixed( &(_THIS->bounds) ),&FX_idx );
7139 
7140  if (_THIS->options.dropEqConPriority <= y_min_priority)
7141  {
7142  /* look for an equality constraint we can drop according to priorities */
7143  for ( i = 0; i < nAC; ++i )
7144  if ( (Constraints_getType( &(_THIS->constraints),i) == ST_EQUALITY)
7145  && (qpOASES_getAbs (xiC[i]) > _THIS->options.epsDen) )
7146  {
7147  y_min_number = AC_idx[i];
7148  y_min_isBound = BT_FALSE;
7149  y_min_priority = _THIS->options.dropEqConPriority;
7150  break;
7151  }
7152  }
7153 
7154  if (_THIS->options.dropIneqConPriority <= y_min_priority)
7155  {
7156  /* look for an inequality constraint we can drop according to priorities */
7157  for ( i = 0; i < nAC; ++i )
7158  if ( (Constraints_getType( &(_THIS->constraints),i) == ST_BOUNDED)
7159  && (qpOASES_getAbs (xiC[i]) > _THIS->options.epsDen) )
7160  {
7161  y_min_number = AC_idx[i];
7162  y_min_isBound = BT_FALSE;
7163  y_min_priority = _THIS->options.dropIneqConPriority;
7164  break;
7165  }
7166  }
7167 
7168  if (_THIS->options.dropBoundPriority <= y_min_priority)
7169  {
7170  /* look for a simple bound we can drop according to priorities */
7171  for ( i = 0; i < nFX; ++i )
7172  if (qpOASES_getAbs (xiB[i]) > _THIS->options.epsDen)
7173  {
7174  y_min_number = FX_idx[i];
7175  y_min_isBound = BT_TRUE;
7176  y_min_priority = _THIS->options.dropBoundPriority;
7177  break;
7178  }
7179  }
7180 
7181  if (y_min_number >= 0) {
7182 
7183  /* drop active equality or active bound we have found */
7184  if (y_min_isBound) {
7185  SubjectToStatus status_ = Bounds_getStatus( &(_THIS->bounds),y_min_number);
7186  QProblem_removeBound( _THIS,y_min_number, BT_TRUE, BT_FALSE, BT_FALSE);
7187  Bounds_setStatus(&(_THIS->bounds),y_min_number, (status_ == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
7188  /* TODO: fix duals _THIS->y[] */
7189  /*fprintf (stdFile, "Dropping bounds %d for %s %d\n", y_min_number, BC_isBound?"bound":"constraint", BC_number);*/
7190  } else {
7191  SubjectToStatus status_ = Constraints_getStatus( &(_THIS->constraints),y_min_number);
7192  QProblem_removeConstraint( _THIS,y_min_number, BT_TRUE, BT_FALSE, BT_FALSE);
7193  Constraints_setStatus( &(_THIS->constraints),y_min_number, (status_ == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
7194  /* TODO: fix duals _THIS->y[] */
7195  /*fprintf (stdFile, "Dropping constraint %d for %s %d\n", y_min_number, BC_isBound?"bound":"constraint", BC_number);*/
7196  }
7197 
7198  /*/ ... now return, add the blocking constraint, and continue solving QP with dropped bound/constraint */
7199  return SUCCESSFUL_RETURN;
7200 
7201  } else {
7202 
7203  /* nothing found, then drop the blocking (still inactive) constraint */
7204  if (BC_isBound)
7205  Bounds_setStatus(&(_THIS->bounds),BC_number, (BC_status == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
7206  else
7207  Constraints_setStatus( &(_THIS->constraints),BC_number, (BC_status == ST_LOWER) ? ST_INFEASIBLE_LOWER : ST_INFEASIBLE_UPPER);
7208 
7209  /*fprintf (stdFile, "Dropping %s %d itself\n", BC_isBound?"bound":"constraint", BC_number);*/
7210 
7211  /* ... now return, and continue solving QP with dropped bound/constraint */
7212  return RET_ENSURELI_DROPPED;
7213  }
7214 }
7215 
7216 
7217 END_NAMESPACE_QPOASES
7218 
7219 
7220 /*
7221  * end of file
7222  */
RET_REMOVEBOUND_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:146
RET_STEPSIZE_NONPOSITIVE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:139
RET_LINEARLY_DEPENDENT
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:153
Bounds_setupBound
returnValue Bounds_setupBound(Bounds *_THIS, int number, SubjectToStatus _status)
Definition: Bounds.c:125
QProblem_performRatioTestB
returnValue QProblem_performRatioTestB(QProblem *_THIS, int nIdx, const int *const idxList, Bounds *const subjectTo, const real_t *const num, const real_t *const den, real_t epsNum, real_t epsDen, real_t *t, int *BC_idx)
Definition: QProblem.c:2133
QProblem::g
real_t g[NVMAX]
Definition: QProblem.h:68
QProblem_loadQPvectorsFromFile
returnValue QProblem_loadQPvectorsFromFile(QProblem *_THIS, const char *const g_file, const char *const lb_file, const char *const ub_file, const char *const lbA_file, const char *const ubA_file, real_t *const g_new, real_t *const lb_new, real_t *const ub_new, real_t *const lbA_new, real_t *const ubA_new)
Definition: QProblem.c:6785
RET_INIT_FAILED_HOTSTART
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:103
QProblem::status
QProblemStatus status
Definition: QProblem.h:82
QProblem_isCPUtimeLimitExceeded
BooleanType QProblem_isCPUtimeLimitExceeded(QProblem *_THIS, const real_t *const cputime, real_t starttime, int nWSR)
Definition: QProblem.c:2064
DenseMatrix_getNorm
real_t DenseMatrix_getNorm(DenseMatrix *_THIS, int type)
Definition: Matrices.c:135
QProblem_getNIAC
static int QProblem_getNIAC(QProblem *_THIS)
Definition: QProblem.h:2088
QProblemBCPY_getRelativeHomotopyLength
real_t QProblemBCPY_getRelativeHomotopyLength(QProblem *_THIS, const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new)
Definition: QProblem.c:2183
Constraints_getNC
static int Constraints_getNC(Constraints *_THIS)
Definition: Constraints.h:444
Bounds_getFree
static Indexlist * Bounds_getFree(Bounds *_THIS)
Definition: Bounds.h:508
QProblem_dropInfeasibles
returnValue QProblem_dropInfeasibles(QProblem *_THIS, int BC_number, SubjectToStatus BC_status, BooleanType BC_isBound, real_t *xiB, real_t *xiC)
Definition: QProblem.c:7121
DenseMatrix_subTimes
returnValue DenseMatrix_subTimes(DenseMatrix *_THIS, const Indexlist *const irows, const Indexlist *const icols, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD, BooleanType yCompr)
Definition: Matrices.c:230
DenseMatrix_diag
real_t DenseMatrix_diag(DenseMatrix *_THIS, int i)
Definition: Matrices.c:108
RET_UNABLE_TO_READ_FILE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:172
QProblemBCPY_setupQPdataFromFile
returnValue QProblemBCPY_setupQPdataFromFile(QProblem *_THIS, const char *const H_file, const char *const g_file, const char *const lb_file, const char *const ub_file)
Definition: QProblem.c:1919
ConstraintProduct
Interface for specifying user-defined evaluations of constraint products.
Definition: qpOASES-3.0beta/include/qpOASES/ConstraintProduct.hpp:57
RET_UNKNOWN_BUG
Definition: acado_types.hpp:845
ST_LOWER
#define ST_LOWER
Definition: qpOASES_wrapper.h:73
Constraints_getNUC
static int Constraints_getNUC(Constraints *_THIS)
Definition: Constraints.h:471
QProblem_hotstartF
returnValue QProblem_hotstartF(QProblem *_THIS, const char *const g_file, const char *const lb_file, const char *const ub_file, const char *const lbA_file, const char *const ubA_file, int *nWSR, real_t *const cputime)
Definition: QProblem.c:759
RET_STEPLENGTH_DETERMINATION_FAILED
Definition: acado_types.hpp:1096
Bounds_hasNoLower
static BooleanType Bounds_hasNoLower(Bounds *_THIS)
Definition: Bounds.h:435
RET_HESSIAN_NOT_SPD
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:164
QProblem_hotstartW
returnValue QProblem_hotstartW(QProblem *_THIS, const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new, int *nWSR, real_t *const cputime, Bounds *const guessedBounds, Constraints *const guessedConstraints)
Definition: QProblem.c:801
QProblem_setupSubjectToType
returnValue QProblem_setupSubjectToType(QProblem *_THIS)
Definition: QProblem.c:2760
QProblemBCPY_updateFarBounds
returnValue QProblemBCPY_updateFarBounds(QProblem *_THIS, real_t curFarBound, int nRamp, const real_t *const lb_new, real_t *const lb_new_far, const real_t *const ub_new, real_t *const ub_new_far)
Definition: QProblem.c:6245
QProblem_removeBound
returnValue QProblem_removeBound(QProblem *_THIS, int number, BooleanType updateCholesky, BooleanType allowFlipping, BooleanType ensureNZC)
Definition: QProblem.c:4796
QProblem_printOptions
returnValue QProblem_printOptions(QProblem *_THIS)
Definition: QProblem.c:1240
Indexlist_getLastNumber
static int Indexlist_getLastNumber(Indexlist *_THIS)
Definition: Indexlist.h:192
Bounds_setNoUpper
static void Bounds_setNoUpper(Bounds *_THIS, BooleanType _status)
Definition: Bounds.h:426
QProblem_setupInitialCholesky
returnValue QProblem_setupInitialCholesky(QProblem *_THIS)
Definition: QProblem.c:544
QProblem_updateFarBounds
returnValue QProblem_updateFarBounds(QProblem *_THIS, real_t curFarBound, int nRamp, const real_t *const lb_new, real_t *const lb_new_far, const real_t *const ub_new, real_t *const ub_new_far, const real_t *const lbA_new, real_t *const lbA_new_far, const real_t *const ubA_new, real_t *const ubA_new_far)
Definition: QProblem.c:6185
RET_SHIFT_DETERMINATION_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:121
__LINE__
#define __LINE__
Definition: acado_message_handling.hpp:98
DenseMatrix_addToDiag
returnValue DenseMatrix_addToDiag(DenseMatrix *_THIS, real_t alpha)
Definition: Matrices.c:474
isZero
BooleanType isZero(real_t x, real_t TOL=ZERO)
QProblem_determineStepDirection
returnValue QProblem_determineStepDirection(QProblem *_THIS, const real_t *const delta_g, const real_t *const delta_lbA, const real_t *const delta_ubA, const real_t *const delta_lb, const real_t *const delta_ub, BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero, real_t *const delta_xFX, real_t *const delta_xFR, real_t *const delta_yAC, real_t *const delta_yFX)
Definition: QProblem.c:5397
QProblem::flipper
Flipper flipper
Definition: QProblem.h:101
QProblem_setupAuxiliaryQPsolution
returnValue QProblem_setupAuxiliaryQPsolution(QProblem *_THIS, const real_t *const xOpt, const real_t *const yOpt)
Definition: QProblem.c:3386
Indexlist_getIndex
int Indexlist_getIndex(Indexlist *_THIS, int givennumber)
Definition: Indexlist.c:129
Bounds_setupAllFree
returnValue Bounds_setupAllFree(Bounds *_THIS)
Definition: Bounds.c:161
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
RET_QPOBJECT_NOT_SETUP
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:92
RET_NO_CHOLESKY_WITH_INITIAL_GUESS
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:115
RET_ITERATION_STARTED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:120
DenseMatrix_bilinear
returnValue DenseMatrix_bilinear(DenseMatrix *_THIS, const Indexlist *const icols, int xN, const real_t *x, int xLD, real_t *y, int yLD)
Definition: Matrices.c:490
RET_HOMOTOPY_STEP_FAILED
Definition: acado_types.hpp:1093
RET_PRINT_ITERATION_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:167
Constraints_getStatus
static SubjectToStatus Constraints_getStatus(Constraints *_THIS, int i)
Definition: Constraints.h:365
Bounds_getStatus
static SubjectToStatus Bounds_getStatus(Bounds *_THIS, int i)
Definition: Bounds.h:375
DenseMatrix_getCol
returnValue DenseMatrix_getCol(DenseMatrix *_THIS, int cNum, const Indexlist *const irows, real_t alpha, real_t *col)
Definition: Matrices.c:178
Constraints_getInactive
static Indexlist * Constraints_getInactive(Constraints *_THIS)
Definition: Constraints.h:508
QProblem_performRatioTestC
returnValue QProblem_performRatioTestC(QProblem *_THIS, int nIdx, const int *const idxList, Constraints *const subjectTo, const real_t *const num, const real_t *const den, real_t epsNum, real_t epsDen, real_t *t, int *BC_idx)
Definition: QProblem.c:6379
RET_MOVING_BOUND_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:94
RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:119
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
RET_NO_DIAGONAL_AVAILABLE
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:201
qpOASES_getMin
static real_t qpOASES_getMin(real_t x, real_t y)
Definition: Utils.h:452
QProblem_setupAuxiliaryQPgradient
returnValue QProblem_setupAuxiliaryQPgradient(QProblem *_THIS)
Definition: QProblem.c:3444
Constraints_setNoUpper
static void Constraints_setNoUpper(Constraints *_THIS, BooleanType _status)
Definition: Constraints.h:416
QQ
#define QQ(I, J)
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:135
RET_MAX_NWSR_REACHED
Definition: acado_types.hpp:1098
QProblem_addConstraint_checkLI
returnValue QProblem_addConstraint_checkLI(QProblem *_THIS, int number)
Definition: QProblem.c:3803
THROWINFO
#define THROWINFO(retval)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:397
ST_INFEASIBLE_UPPER
#define ST_INFEASIBLE_UPPER
Definition: qpOASES_wrapper.h:77
TT
#define TT(I, J)
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:138
QProblem_initM
returnValue QProblem_initM(QProblem *_THIS, DenseMatrix *_H, const real_t *const _g, DenseMatrix *_A, const real_t *const _lb, const real_t *const _ub, const real_t *const _lbA, const real_t *const _ubA, int *nWSR, real_t *const cputime)
Definition: QProblem.c:256
QProblem_backsolveT
returnValue QProblem_backsolveT(QProblem *_THIS, const real_t *const b, BooleanType transposed, real_t *const a)
Definition: QProblem.c:5280
NCMAX
#define NCMAX
Definition: qpoases3/include/qpOASES_e/Constants.h:74
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
RET_STEPDIRECTION_FAILED_TQ
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:133
QProblem_areBoundsConsistent
returnValue QProblem_areBoundsConsistent(QProblem *_THIS, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new)
Definition: QProblem.c:7090
RET_INDEXLIST_CORRUPTED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:83
HST_POSDEF
#define HST_POSDEF
Definition: qpOASES_wrapper.h:66
QProblem_solveRegularisedQP
returnValue QProblem_solveRegularisedQP(QProblem *_THIS, const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new, int *nWSR, real_t *const cputime, int nWSRperformed, BooleanType isFirstCall)
Definition: QProblem.c:2655
DenseMatrix_getRow
returnValue DenseMatrix_getRow(DenseMatrix *_THIS, int rNum, const Indexlist *const icols, real_t alpha, real_t *row)
Definition: Matrices.c:147
Constraints_setNoLower
static void Constraints_setNoLower(Constraints *_THIS, BooleanType _status)
Definition: Constraints.h:407
QProblem::hessianType
HessianType hessianType
Definition: QProblem.h:87
QProblem_setupAuxiliaryQPbounds
returnValue QProblem_setupAuxiliaryQPbounds(QProblem *_THIS, Bounds *const auxiliaryBounds, Constraints *const auxiliaryConstraints, BooleanType useRelaxation)
Definition: QProblem.c:3489
RET_INIT_FAILED_REGULARISATION
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:102
RR
#define RR(I, J)
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:132
QProblem_getNFR
static int QProblem_getNFR(QProblem *_THIS)
Definition: QProblem.h:1700
Options_setToFast
returnValue Options_setToFast(Options *_THIS)
Definition: Options.c:252
DenseMatrix_transTimes
returnValue DenseMatrix_transTimes(DenseMatrix *_THIS, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD)
Definition: Matrices.c:213
QProblemBCPY_setupQPdata
returnValue QProblemBCPY_setupQPdata(QProblem *_THIS, real_t *const _H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub)
Definition: QProblem.c:1895
NVMAX
#define NVMAX
Definition: qpoases3/include/qpOASES_e/Constants.h:70
PL_MEDIUM
#define PL_MEDIUM
Definition: qpOASES_wrapper.h:85
Bounds_hasNoUpper
static BooleanType Bounds_hasNoUpper(Bounds *_THIS)
Definition: Bounds.h:444
RET_ADDCONSTRAINT_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:141
QProblem_setConstraintProduct
returnValue QProblem_setConstraintProduct(QProblem *_THIS, ConstraintProduct _constraintProduct)
Definition: QProblem.c:1084
THROWERROR
#define THROWERROR(retval)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:391
MessageHandling_throwWarning
returnValue MessageHandling_throwWarning(MessageHandling *_THIS, returnValue Wnumber, const char *additionaltext, const char *functionname, const char *filename, const unsigned long linenumber, VisibilityStatus localVisibilityStatus)
Definition: MessageHandling.c:293
VS_HIDDEN
Definition: acado_message_handling.hpp:54
QProblem_solveCurrentEQP
returnValue QProblem_solveCurrentEQP(QProblem *_THIS, const int n_rhs, const real_t *g_in, const real_t *lb_in, const real_t *ub_in, const real_t *lbA_in, const real_t *ubA_in, real_t *x_out, real_t *y_out)
Definition: QProblem.c:910
QProblem_getNZ
int QProblem_getNZ(QProblem *_THIS)
Definition: QProblem.c:1048
RET_INIT_FAILED_UNBOUNDEDNESS
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:105
QProblem_computeProjectedCholesky
returnValue QProblem_computeProjectedCholesky(QProblem *_THIS)
Definition: QProblem.c:2917
Constraints_setupConstraint
returnValue Constraints_setupConstraint(Constraints *_THIS, int number, SubjectToStatus _status)
Definition: Constraints.c:127
HessianType
HessianType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:116
QPS_SOLVED
Definition: acado_types.hpp:674
QProblem_addConstraint
returnValue QProblem_addConstraint(QProblem *_THIS, int number, SubjectToStatus C_status, BooleanType updateCholesky, BooleanType ensureLI)
Definition: QProblem.c:3639
ST_INFEASIBLE_LOWER
#define ST_INFEASIBLE_LOWER
Definition: qpOASES_wrapper.h:76
QProblem_isInfeasible
static BooleanType QProblem_isInfeasible(QProblem *_THIS)
Definition: QProblem.h:1760
QProblem_getNAC
static int QProblem_getNAC(QProblem *_THIS)
Definition: QProblem.h:2079
QProblemBCPY_determineDataShift
returnValue QProblemBCPY_determineDataShift(QProblem *_THIS, const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, real_t *const delta_g, real_t *const delta_lb, real_t *const delta_ub, BooleanType *Delta_bB_isZero)
Definition: QProblem.c:1818
QProblem_solveInitialQP
returnValue QProblem_solveInitialQP(QProblem *_THIS, const real_t *const xOpt, const real_t *const yOpt, Bounds *const guessedBounds, Constraints *const guessedConstraints, const real_t *const _R, int *nWSR, real_t *const cputime)
Definition: QProblem.c:2232
HST_IDENTITY
#define HST_IDENTITY
Definition: qpOASES_wrapper.h:65
QPOASES_ZERO
static const real_t QPOASES_ZERO
Definition: qpoases3/include/qpOASES_e/Constants.h:110
Flipper_set
returnValue Flipper_set(Flipper *_THIS, const Bounds *const _bounds, const real_t *const _R, const Constraints *const _constraints, const real_t *const _Q, const real_t *const _T)
Definition: Flipper.c:119
Bounds_moveFreeToFixed
returnValue Bounds_moveFreeToFixed(Bounds *_THIS, int number, SubjectToStatus _status)
Definition: Bounds.c:208
RET_ADD_TO_ACTIVESET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:150
DenseMatrix_isDiag
BooleanType DenseMatrix_isDiag(DenseMatrix *_THIS)
Definition: Matrices.c:119
MessageHandling_throwInfo
returnValue MessageHandling_throwInfo(MessageHandling *_THIS, returnValue Inumber, const char *additionaltext, const char *functionname, const char *filename, const unsigned long linenumber, VisibilityStatus localVisibilityStatus)
Definition: MessageHandling.c:317
PL_NONE
#define PL_NONE
Definition: qpOASES_wrapper.h:83
RET_REMOVECONSTRAINT_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:145
qpOASES_getCPUtime
real_t qpOASES_getCPUtime()
Definition: Utils.c:606
qpOASES_readFromFileV
returnValue qpOASES_readFromFileV(real_t *data, int n, const char *datafilename)
Definition: Utils.c:367
QProblemBCPY_computeCholesky
returnValue QProblemBCPY_computeCholesky(QProblem *_THIS)
Definition: QProblem.c:1525
Flipper_get
returnValue Flipper_get(Flipper *_THIS, Bounds *const _bounds, real_t *const R, Constraints *const _constraints, real_t *const _Q, real_t *const _T)
Definition: Flipper.c:89
RET_LI_RESOLVED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:155
QProblem::ramp1
real_t ramp1
Definition: QProblem.h:95
ST_UNDEFINED
#define ST_UNDEFINED
Definition: qpOASES_wrapper.h:78
QProblem_determineDataShift
returnValue QProblem_determineDataShift(QProblem *_THIS, const real_t *const g_new, const real_t *const lbA_new, const real_t *const ubA_new, const real_t *const lb_new, const real_t *const ub_new, real_t *const delta_g, real_t *const delta_lbA, real_t *const delta_ubA, real_t *const delta_lb, real_t *const delta_ub, BooleanType *Delta_bC_isZero, BooleanType *Delta_bB_isZero)
Definition: QProblem.c:5333
Bounds_setNoLower
static void Bounds_setNoLower(Bounds *_THIS, BooleanType _status)
Definition: Bounds.h:417
RET_CANNOT_REGULARISE_IDENTITY
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:176
MessageHandling_reset
returnValue MessageHandling_reset(MessageHandling *_THIS)
Definition: MessageHandling.c:341
RET_HESSIAN_INDEFINITE
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:162
QProblem_setupTQfactorisation
returnValue QProblem_setupTQfactorisation(QProblem *_THIS)
Definition: QProblem.c:3011
ST_INACTIVE
#define ST_INACTIVE
Definition: qpOASES_wrapper.h:74
QProblemBCPY_setupQPdataM
returnValue QProblemBCPY_setupQPdataM(QProblem *_THIS, DenseMatrix *_H, const real_t *const _g, const real_t *const _lb, const real_t *const _ub)
Definition: QProblem.c:1881
QProblem_setupSubjectToTypeNew
returnValue QProblem_setupSubjectToTypeNew(QProblem *_THIS, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new)
Definition: QProblem.c:2769
QProblem_addBound_checkLI
returnValue QProblem_addBound_checkLI(QProblem *_THIS, int number)
Definition: QProblem.c:4283
Constraints_moveInactiveToActive
returnValue Constraints_moveInactiveToActive(Constraints *_THIS, int number, SubjectToStatus _status)
Definition: Constraints.c:216
RET_QP_INFEASIBLE
Definition: acado_types.hpp:1081
NVCMIN
#define NVCMIN
Definition: qpoases3/include/qpOASES_e/Constants.h:85
QProblem_getWorkingSetBounds
returnValue QProblem_getWorkingSetBounds(QProblem *_THIS, real_t *workingSetB)
Definition: QProblem.c:1001
QProblem_getNV
static int QProblem_getNV(QProblem *_THIS)
Definition: QProblem.h:1691
DenseMatrix_subTransTimes
returnValue DenseMatrix_subTransTimes(DenseMatrix *_THIS, const Indexlist *const irows, const Indexlist *const icols, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD)
Definition: Matrices.c:416
QProblem_printProperties
returnValue QProblem_printProperties(QProblem *_THIS)
Definition: QProblem.c:1249
QProblem_setUBA
static returnValue QProblem_setUBA(QProblem *_THIS, const real_t *const ubA_new)
Definition: QProblem.h:2196
RET_STEPDIRECTION_DETERMINATION_FAILED
Definition: acado_types.hpp:1094
QProblem_ensureNonzeroCurvature
returnValue QProblem_ensureNonzeroCurvature(QProblem *_THIS, BooleanType removeBoundNotConstraint, int remIdx, BooleanType *exchangeHappened, BooleanType *addBoundNotConstraint, int *addIdx, SubjectToStatus *addStatus)
Definition: QProblem.c:5050
QProblem_removeConstraint
returnValue QProblem_removeConstraint(QProblem *_THIS, int number, BooleanType updateCholesky, BooleanType allowFlipping, BooleanType ensureNZC)
Definition: QProblem.c:4565
QProblem_isInitialised
static BooleanType QProblem_isInitialised(QProblem *_THIS)
Definition: QProblem.h:1736
qpOASES_readFromFileM
returnValue qpOASES_readFromFileM(real_t *data, int nrow, int ncol, const char *datafilename)
Definition: Utils.c:314
Bounds_init
returnValue Bounds_init(Bounds *_THIS, int _n)
Definition: Bounds.c:87
HST_INDEF
#define HST_INDEF
Definition: qpOASES_wrapper.h:69
RET_SETUP_WORKINGSET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:108
RET_QP_NOT_SOLVED
Definition: acado_types.hpp:1083
QProblem::rampOffset
int rampOffset
Definition: QProblem.h:96
Bounds_setType
static returnValue Bounds_setType(Bounds *_THIS, int i, SubjectToType value)
Definition: Bounds.h:387
DenseMatrixCON
void DenseMatrixCON(DenseMatrix *_THIS, int m, int n, int lD, real_t *v)
Definition: Matrices.c:47
PL_HIGH
#define PL_HIGH
Definition: qpOASES_wrapper.h:86
Options_print
returnValue Options_print(Options *_THIS)
Definition: Options.c:406
MessageHandling_setInfoVisibilityStatus
static void MessageHandling_setInfoVisibilityStatus(MessageHandling *_THIS, VisibilityStatus _infoVisibility)
Definition: MessageHandling.h:511
ST_UNBOUNDED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:89
ConstraintsCPY
void ConstraintsCPY(Constraints *FROM, Constraints *TO)
Definition: Constraints.c:61
RET_CHOLESKY_OF_ZERO_HESSIAN
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:180
ST_BOUNDED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:90
QProblem::regVal
real_t regVal
Definition: QProblem.h:88
QProblem::ramp0
real_t ramp0
Definition: QProblem.h:94
DenseMatrix
Interfaces matrix-vector operations tailored to general dense matrices.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:273
qpOASES_getAbs
static real_t qpOASES_getAbs(real_t x)
Definition: Utils.h:468
QProblem_initF
returnValue QProblem_initF(QProblem *_THIS, const char *const H_file, const char *const g_file, const char *const A_file, const char *const lb_file, const char *const ub_file, const char *const lbA_file, const char *const ubA_file, int *nWSR, real_t *const cputime)
Definition: QProblem.c:327
QProblem::bounds
Bounds bounds
Definition: QProblem.h:72
QProblem_getDualSolution
returnValue QProblem_getDualSolution(QProblem *_THIS, real_t *const yOpt)
Definition: QProblem.c:1058
ST_DISABLED
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:262
qpOASES_getMinI
static int qpOASES_getMinI(int x, int y)
Definition: Utils.h:425
Constraints_getActive
static Indexlist * Constraints_getActive(Constraints *_THIS)
Definition: Constraints.h:499
BooleanType
BooleanType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:44
RET_CONSTRAINT_NOT_ACTIVE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:162
QPS_AUXILIARYQPSOLVED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:77
QProblemBCPY_obtainAuxiliaryWorkingSet
returnValue QProblemBCPY_obtainAuxiliaryWorkingSet(QProblem *_THIS, const real_t *const xOpt, const real_t *const yOpt, Bounds *const guessedBounds, Bounds *auxiliaryBounds)
Definition: QProblem.c:1604
QProblem::HH
DenseMatrix HH
Definition: QProblem.h:66
Constraints_getType
static SubjectToType Constraints_getType(Constraints *_THIS, int i)
Definition: Constraints.h:353
RET_CANNOT_REGULARISE_SPARSE
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:177
QProblem_setG
static returnValue QProblem_setG(QProblem *_THIS, const real_t *const g_new)
Definition: QProblem.h:1905
QProblem::count
unsigned int count
Definition: QProblem.h:90
nu
const int nu
Definition: rocket_with_templates_ocp.hpp:38
qpOASES_getMax
static real_t qpOASES_getMax(real_t x, real_t y)
Definition: Utils.h:436
QProblem_setInfeasibilityFlag
returnValue QProblem_setInfeasibilityFlag(QProblem *_THIS, returnValue returnvalue, BooleanType doThrowError)
Definition: QProblem.c:2048
Bounds_getNFX
static int Bounds_getNFX(Bounds *_THIS)
Definition: Bounds.h:499
RET_ENSURELI_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:156
QProblem_addBound
returnValue QProblem_addBound(QProblem *_THIS, int number, SubjectToStatus B_status, BooleanType updateCholesky, BooleanType ensureLI)
Definition: QProblem.c:4107
QProblemB_applyGivens
static void QProblemB_applyGivens(real_t c, real_t s, real_t nu, real_t xold, real_t yold, real_t *xnew, real_t *ynew)
Definition: QProblemB.h:1541
PL_DEBUG_ITER
#define PL_DEBUG_ITER
Definition: qpOASES_wrapper.h:81
Indexlist_getLength
static int Indexlist_getLength(Indexlist *_THIS)
Definition: Indexlist.h:183
QProblemCON
BEGIN_NAMESPACE_QPOASES void QProblemCON(QProblem *_THIS, int _nV, int _nC, HessianType _hessianType)
Definition: QProblem.c:51
QProblem_getWorkingSet
returnValue QProblem_getWorkingSet(QProblem *_THIS, real_t *workingSet)
Definition: QProblem.c:981
RET_ENSURELI_FAILED_TQ
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:157
RET_HOTSTART_STOPPED_UNBOUNDEDNESS
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:127
qpOASES_myPrintf
returnValue qpOASES_myPrintf(const char *s)
Definition: Utils.c:269
QProblem::x
real_t x[NVMAX]
Definition: QProblem.h:77
Bounds_getNUV
static int Bounds_getNUV(Bounds *_THIS)
Definition: Bounds.h:481
QProblem::AA
DenseMatrix AA
Definition: QProblem.h:104
RET_INIT_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:100
QProblem::infeasible
BooleanType infeasible
Definition: QProblem.h:84
RET_PRINTLEVEL_CHANGED
Definition: acado_types.hpp:846
QProblem::tabularOutput
TabularOutput tabularOutput
Definition: QProblem.h:99
QProblem_reset
returnValue QProblem_reset(QProblem *_THIS)
Definition: QProblem.c:202
QPS_PERFORMINGHOMOTOPY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:79
assert
#define assert(ignore)
Definition: MessageHandling.h:46
QProblem_getNFX
static int QProblem_getNFX(QProblem *_THIS)
Definition: QProblem.h:1709
QProblem_backsolveRrem
returnValue QProblem_backsolveRrem(QProblem *_THIS, const real_t *const b, BooleanType transposed, BooleanType removingBound, real_t *const a)
Definition: QProblem.c:1760
QProblem_hotstart
returnValue QProblem_hotstart(QProblem *_THIS, const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new, int *nWSR, real_t *const cputime)
Definition: QProblem.c:578
qpOASES_getSqrt
static real_t qpOASES_getSqrt(real_t x)
Definition: Utils.h:481
qpOASES_getNorm
real_t qpOASES_getNorm(const real_t *const v, int n, int type)
Definition: Utils.c:629
QProblem_getNFV
static int QProblem_getNFV(QProblem *_THIS)
Definition: QProblem.h:1718
RET_ADDBOUND_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:143
QProblem::options
Options options
Definition: QProblem.h:98
RET_NO_REGSTEP_NWSR
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:178
SubjectToStatus
SubjectToStatus
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:97
RET_INIT_FAILED_CHOLESKY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:102
QProblem_setH
static returnValue QProblem_setH(QProblem *_THIS, real_t *const H_new)
Definition: QProblem.h:1880
PL_LOW
#define PL_LOW
Definition: qpOASES_wrapper.h:84
Constraints_moveActiveToInactive
returnValue Constraints_moveActiveToInactive(Constraints *_THIS, int number)
Definition: Constraints.c:194
QProblemBCPY_loadQPvectorsFromFile
returnValue QProblemBCPY_loadQPvectorsFromFile(QProblem *_THIS, const char *const g_file, const char *const lb_file, const char *const ub_file, real_t *const g_new, real_t *const lb_new, real_t *const ub_new)
Definition: QProblem.c:1987
VS_VISIBLE
Definition: acado_message_handling.hpp:53
QProblem_printIteration
returnValue QProblem_printIteration(QProblem *_THIS, int iter, int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound, real_t homotopyLength, BooleanType isFirstCall)
Definition: QProblem.c:6844
QPOASES_INFTY
static const real_t QPOASES_INFTY
Definition: qpoases3/include/qpOASES_e/Constants.h:114
RET_ALL_BOUNDS_ACTIVE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:161
RET_DIAGONAL_NOT_INITIALISED
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:217
RET_ALL_CONSTRAINTS_ACTIVE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:152
QProblem_getStatus
static QProblemStatus QProblem_getStatus(QProblem *_THIS)
Definition: QProblem.h:1727
QProblem::tau
real_t tau
Definition: QProblem.h:80
MessageHandling_setErrorVisibilityStatus
static void MessageHandling_setErrorVisibilityStatus(MessageHandling *_THIS, VisibilityStatus _errorVisibility)
Definition: MessageHandling.h:493
rhs
void rhs(const real_t *x, real_t *f)
Definition: examples/code_generation/simulation/externModel_export/model.c:5
RET_BOUND_NOT_ACTIVE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:163
MessageHandling_setWarningVisibilityStatus
static void MessageHandling_setWarningVisibilityStatus(MessageHandling *_THIS, VisibilityStatus _warningVisibility)
Definition: MessageHandling.h:502
RET_LINEARLY_INDEPENDENT
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:154
QProblem_setupAuxiliaryQP
returnValue QProblem_setupAuxiliaryQP(QProblem *_THIS, Bounds *const guessedBounds, Constraints *const guessedConstraints)
Definition: QProblem.c:6528
END_NAMESPACE_QPOASES
#define END_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:120
QProblem_obtainAuxiliaryWorkingSet
returnValue QProblem_obtainAuxiliaryWorkingSet(QProblem *_THIS, const real_t *const xOpt, const real_t *const yOpt, Bounds *const guessedBounds, Constraints *const guessedConstraints, Bounds *auxiliaryBounds, Constraints *auxiliaryConstraints)
Definition: QProblem.c:3040
PrintLevel
PrintLevel
Definition: acado_types.hpp:292
Constraints_init
returnValue Constraints_init(Constraints *_THIS, int _n)
Definition: Constraints.c:89
RET_QP_ALREADY_INITIALISED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:97
Bounds_moveFixedToFree
returnValue Bounds_moveFixedToFree(Bounds *_THIS, int number)
Definition: Bounds.c:188
QProblem_changeActiveSet
returnValue QProblem_changeActiveSet(QProblem *_THIS, int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound)
Definition: QProblem.c:6055
OptionsCPY
void OptionsCPY(Options *FROM, Options *TO)
Definition: Options.c:64
RET_ERROR_IN_CONSTRAINTPRODUCT
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:182
RET_USING_REGULARISATION
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:172
QProblem_setLB
static returnValue QProblem_setLB(QProblem *_THIS, const real_t *const lb_new)
Definition: QProblem.h:1924
QProblem::y
real_t y[NVMAX+NCMAX]
Definition: QProblem.h:78
QProblem_getWorkingSetConstraints
returnValue QProblem_getWorkingSetConstraints(QProblem *_THIS, real_t *workingSetC)
Definition: QProblem.c:1022
QProblem_initFW
returnValue QProblem_initFW(QProblem *_THIS, const char *const H_file, const char *const g_file, const char *const A_file, const char *const lb_file, const char *const ub_file, const char *const lbA_file, const char *const ubA_file, int *nWSR, real_t *const cputime, const real_t *const xOpt, const real_t *const yOpt, Bounds *const guessedBounds, Constraints *const guessedConstraints, const char *const R_file)
Definition: QProblem.c:472
RET_ADDBOUND_FAILED_INFEASIBILITY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:144
QProblem_getRelativeHomotopyLength
real_t QProblem_getRelativeHomotopyLength(QProblem *_THIS, const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new)
Definition: QProblem.c:6142
RET_CONSTRAINT_ALREADY_ACTIVE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:151
QProblem_shallRefactorise
BooleanType QProblem_shallRefactorise(QProblem *_THIS, Bounds *const guessedBounds, Constraints *const guessedConstraints)
Definition: QProblem.c:6617
HST_POSDEF_NULLSPACE
#define HST_POSDEF_NULLSPACE
Definition: qpOASES_wrapper.h:67
QProblem_getNC
static int QProblem_getNC(QProblem *_THIS)
Definition: QProblem.h:2061
qpOASES_getGlobalMessageHandler
MessageHandling * qpOASES_getGlobalMessageHandler()
Definition: MessageHandling.c:545
RET_ADD_TO_ACTIVESET
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:148
Bounds_flipFixed
returnValue Bounds_flipFixed(Bounds *_THIS, int number)
Definition: Bounds.c:229
QProblem_setupQPdataFromFile
returnValue QProblem_setupQPdataFromFile(QProblem *_THIS, const char *const H_file, const char *const g_file, const char *const A_file, const char *const lb_file, const char *const ub_file, const char *const lbA_file, const char *const ubA_file)
Definition: QProblem.c:6718
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
QProblem_setLBA
static returnValue QProblem_setLBA(QProblem *_THIS, const real_t *const lbA_new)
Definition: QProblem.h:2148
DenseMatrix_getVal
static real_t * DenseMatrix_getVal(DenseMatrix *_THIS)
Definition: Matrices.h:257
Options_setToDefault
returnValue Options_setToDefault(Options *_THIS)
Definition: Options.c:112
RET_OPTIMAL_SOLUTION_FOUND
Definition: acado_types.hpp:1097
RET_SETUP_AUXILIARYQP_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:109
RET_REMOVE_FROM_ACTIVESET
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:147
RET_INIT_SUCCESSFUL
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:106
Constraints_setStatus
static returnValue Constraints_setStatus(Constraints *_THIS, int i, SubjectToStatus value)
Definition: Constraints.h:392
QProblem::delta_xFR_TMP
real_t delta_xFR_TMP[NVMAX]
Definition: QProblem.h:92
ST_UNKNOWN
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:92
HST_ZERO
#define HST_ZERO
Definition: qpOASES_wrapper.h:64
QProblem_isBlocking
static BooleanType QProblem_isBlocking(QProblem *_THIS, real_t num, real_t den, real_t epsNum, real_t epsDen, real_t *t)
Definition: QProblem.h:2022
QProblem_hotstartFW
returnValue QProblem_hotstartFW(QProblem *_THIS, const char *const g_file, const char *const lb_file, const char *const ub_file, const char *const lbA_file, const char *const ubA_file, int *nWSR, real_t *const cputime, Bounds *const guessedBounds, Constraints *const guessedConstraints)
Definition: QProblem.c:864
THROWWARNING
#define THROWWARNING(retval)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:394
RET_INIT_FAILED_TQ
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:101
Constraints_flipFixed
returnValue Constraints_flipFixed(Constraints *_THIS, int number)
Definition: Constraints.c:238
RET_OBTAINING_WORKINGSET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:107
QProblem_regulariseHessian
returnValue QProblem_regulariseHessian(QProblem *_THIS)
Definition: QProblem.c:2094
HST_SEMIDEF
#define HST_SEMIDEF
Definition: qpOASES_wrapper.h:68
QPOASES_EPS
static const real_t QPOASES_EPS
Definition: qpoases3/include/qpOASES_e/Constants.h:101
QProblem_backsolveR
returnValue QProblem_backsolveR(QProblem *_THIS, const real_t *const b, BooleanType transposed, real_t *const a)
Definition: QProblem.c:1748
RET_STEPSIZE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:138
QPS_NOTINITIALISED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:74
QPS_HOMOTOPYQPSOLVED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:81
QProblem_addConstraint_ensureLI
returnValue QProblem_addConstraint_ensureLI(QProblem *_THIS, int number, SubjectToStatus C_status)
Definition: QProblem.c:3925
HST_UNKNOWN
#define HST_UNKNOWN
Definition: qpOASES_wrapper.h:70
QProblem::R
real_t R[NVMAX *NVMAX]
Definition: QProblem.h:74
Constraints_hasNoLower
static BooleanType Constraints_hasNoLower(Constraints *_THIS)
Definition: Constraints.h:425
RET_ZERO_HESSIAN_ASSUMED
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:190
QProblem_initW
returnValue QProblem_initW(QProblem *_THIS, real_t *const _H, const real_t *const _g, real_t *const _A, const real_t *const _lb, const real_t *const _ub, const real_t *const _lbA, const real_t *const _ubA, int *nWSR, real_t *const cputime, const real_t *const xOpt, const real_t *const yOpt, Bounds *const guessedBounds, Constraints *const guessedConstraints, const real_t *const _R)
Definition: QProblem.c:414
Indexlist_getNumberArray
returnValue Indexlist_getNumberArray(Indexlist *_THIS, int **const numberarray)
Definition: Indexlist.c:105
QPS_PREPARINGAUXILIARYQP
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:75
QProblem::H
DenseMatrix * H
Definition: QProblem.h:65
QProblem::ub
real_t ub[NVMAX]
Definition: QProblem.h:70
QProblem::lb
real_t lb[NVMAX]
Definition: QProblem.h:69
RET_INIT_FAILED_INFEASIBILITY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:104
__FILE__
#define __FILE__
Definition: acado_message_handling.hpp:94
QProblem_addBound_ensureLI
returnValue QProblem_addBound_ensureLI(QProblem *_THIS, int number, SubjectToStatus B_status)
Definition: QProblem.c:4388
QPOASES_MAX_STRING_LENGTH
#define QPOASES_MAX_STRING_LENGTH
Definition: qpoases3/include/qpOASES_e/Constants.h:123
QProblem_setupQPdataM
returnValue QProblem_setupQPdataM(QProblem *_THIS, DenseMatrix *_H, const real_t *const _g, DenseMatrix *_A, const real_t *const _lb, const real_t *const _ub, const real_t *const _lbA, const real_t *const _ubA)
Definition: QProblem.c:6656
QProblem_performDriftCorrection
returnValue QProblem_performDriftCorrection(QProblem *_THIS)
Definition: QProblem.c:6429
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
Constraints_setType
static returnValue Constraints_setType(Constraints *_THIS, int i, SubjectToType value)
Definition: Constraints.h:377
PL_TABULAR
#define PL_TABULAR
Definition: qpOASES_wrapper.h:82
ST_EQUALITY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:91
NVCMAX
#define NVCMAX
Definition: qpoases3/include/qpOASES_e/Constants.h:79
RET_ENSURELI_DROPPED
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:219
Bounds
Manages working sets of bounds (= box constraints).
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Bounds.hpp:50
QProblem_solveQP
returnValue QProblem_solveQP(QProblem *_THIS, const real_t *const g_new, const real_t *const lb_new, const real_t *const ub_new, const real_t *const lbA_new, const real_t *const ubA_new, int *nWSR, real_t *const cputime, int nWSRperformed, BooleanType isFirstCall)
Definition: QProblem.c:2414
ST_UPPER
#define ST_UPPER
Definition: qpOASES_wrapper.h:75
BEGIN_NAMESPACE_QPOASES
#define BEGIN_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:117
__FUNC__
#define __FUNC__
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:450
QProblem_initMW
returnValue QProblem_initMW(QProblem *_THIS, DenseMatrix *_H, const real_t *const _g, DenseMatrix *_A, const real_t *const _lb, const real_t *const _ub, const real_t *const _lbA, const real_t *const _ubA, int *nWSR, real_t *const cputime, const real_t *const xOpt, const real_t *const yOpt, Bounds *const guessedBounds, Constraints *const guessedConstraints, const real_t *const _R)
Definition: QProblem.c:355
DenseMatrix_times
returnValue DenseMatrix_times(DenseMatrix *_THIS, int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD)
Definition: Matrices.c:196
real_t
double real_t
Definition: AD_test.c:10
RET_BOUND_ALREADY_ACTIVE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:160
QProblem_determineHessianType
returnValue QProblem_determineHessianType(QProblem *_THIS)
Definition: QProblem.c:1421
RET_ENSURELI_FAILED_NOINDEX
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:158
QProblem_performPlainRatioTest
returnValue QProblem_performPlainRatioTest(QProblem *_THIS, int nIdx, const int *const idxList, const real_t *const num, const real_t *const den, real_t epsNum, real_t epsDen, real_t *t, int *BC_idx)
Definition: QProblem.c:5027
QProblem_getObjValX
real_t QProblem_getObjValX(QProblem *_THIS, const real_t *const _x)
Definition: QProblem.c:1119
QProblem::haveCholesky
BooleanType haveCholesky
Definition: QProblem.h:75
QProblem_usingRegularisation
static BooleanType QProblem_usingRegularisation(QProblem *_THIS)
Definition: QProblem.h:1797
QProblem_getPrimalSolution
returnValue QProblem_getPrimalSolution(QProblem *_THIS, real_t *const xOpt)
Definition: QProblem.c:1168
RET_STEPDIRECTION_FAILED_CHOLESKY
Definition: acado_types.hpp:1095
QProblem
Implements the online active set strategy for QPs with general constraints.
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/QProblem.hpp:53
QProblemB_computeGivens
static void QProblemB_computeGivens(real_t xold, real_t yold, real_t *xnew, real_t *ynew, real_t *c, real_t *s)
Definition: QProblemB.h:1503
QProblem_setUB
static returnValue QProblem_setUB(QProblem *_THIS, const real_t *const ub_new)
Definition: QProblem.h:1972
qpOASES_printCopyrightNotice
returnValue qpOASES_printCopyrightNotice()
Definition: Utils.c:296
Bounds_getType
static SubjectToType Bounds_getType(Bounds *_THIS, int i)
Definition: Bounds.h:363
QProblemCPY
void QProblemCPY(QProblem *FROM, QProblem *TO)
Definition: QProblem.c:132
Bounds_swapFree
returnValue Bounds_swapFree(Bounds *_THIS, int number1, int number2)
Definition: Bounds.c:250
Bounds_setStatus
static returnValue Bounds_setStatus(Bounds *_THIS, int i, SubjectToStatus value)
Definition: Bounds.h:402
QProblem_performStep
returnValue QProblem_performStep(QProblem *_THIS, const real_t *const delta_g, const real_t *const delta_lbA, const real_t *const delta_ubA, const real_t *const delta_lb, const real_t *const delta_ub, const real_t *const delta_xFX, const real_t *const delta_xFR, const real_t *const delta_yAC, const real_t *const delta_yFX, int *BC_idx, SubjectToStatus *BC_status, BooleanType *BC_isBound)
Definition: QProblem.c:5741
QProblem_setupQPdata
returnValue QProblem_setupQPdata(QProblem *_THIS, real_t *const _H, const real_t *const _g, real_t *const _A, const real_t *const _lb, const real_t *const _ub, const real_t *const _lbA, const real_t *const _ubA)
Definition: QProblem.c:6682
Constraints_hasNoUpper
static BooleanType Constraints_hasNoUpper(Constraints *_THIS)
Definition: Constraints.h:434
QProblem_init
returnValue QProblem_init(QProblem *_THIS, real_t *const _H, const real_t *const _g, real_t *const _A, const real_t *const _lb, const real_t *const _ub, const real_t *const _lbA, const real_t *const _ubA, int *nWSR, real_t *const cputime)
Definition: QProblem.c:299
RET_DIV_BY_ZERO
Definition: acado_types.hpp:837
RET_REMOVE_FROM_ACTIVESET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:149
QProblem_setA
static returnValue QProblem_setA(QProblem *_THIS, real_t *const A_new)
Definition: QProblem.h:2115
ConstraintsCON
void ConstraintsCON(Constraints *_THIS, int _n)
Definition: Constraints.c:50
Bounds_getFixed
static Indexlist * Bounds_getFixed(Bounds *_THIS)
Definition: Bounds.h:517
QProblem::unbounded
BooleanType unbounded
Definition: QProblem.h:85
QProblem_getNEC
static int QProblem_getNEC(QProblem *_THIS)
Definition: QProblem.h:2070
QProblem_getObjVal
real_t QProblem_getObjVal(QProblem *_THIS)
Definition: QProblem.c:1095
QProblem_setPrintLevel
returnValue QProblem_setPrintLevel(QProblem *_THIS, PrintLevel _printLevel)
Definition: QProblem.c:1193
RET_ADDCONSTRAINT_FAILED_INFEASIBILITY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:142
QProblem_setupAuxiliaryWorkingSet
returnValue QProblem_setupAuxiliaryWorkingSet(QProblem *_THIS, Bounds *const auxiliaryBounds, Constraints *const auxiliaryConstraints, BooleanType setupAfresh)
Definition: QProblem.c:3192
myStatic
#define myStatic
Definition: Types.h:103
RET_FEWER_REGSTEPS_NWSR
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:179
RET_HOTSTART_STOPPED_INFEASIBILITY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:126
QProblem_performRamping
returnValue QProblem_performRamping(QProblem *_THIS)
Definition: QProblem.c:6296
BoundsCON
void BoundsCON(Bounds *_THIS, int _n)
Definition: Bounds.c:50
Constraints_getNAC
static int Constraints_getNAC(Constraints *_THIS)
Definition: Constraints.h:480
Constraints_setupAllInactive
returnValue Constraints_setupAllInactive(Constraints *_THIS)
Definition: Constraints.c:164
QProblem_isUnbounded
static BooleanType QProblem_isUnbounded(QProblem *_THIS)
Definition: QProblem.h:1769
RET_ENSURELI_FAILED_CYCLING
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:159

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