SQProblemSchur.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of qpOASES.
3  *
4  * qpOASES -- An Implementation of the Online Active Set Strategy.
5  * Copyright (C) 2007-2014 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 
37 #include <qpOASES/SQProblemSchur.hpp>
38 
39 
40 #ifndef __MATLAB__
41 # include <cstdarg>
42 void MyPrintf(const char* pformat, ... )
43 {
44  va_list ap;
45  va_start(ap, pformat);
46 
47  vfprintf(stdout, pformat, ap);
48 
49  va_end(ap);
50 }
51 #else
52 # include <mex.h>
53 # define MyPrintf mexPrintf
54 #endif
55 
56 
57 BEGIN_NAMESPACE_QPOASES
58 
59 
60 /*****************************************************************************
61  * P U B L I C *
62  *****************************************************************************/
63 
64 
65 /*
66  * Q P r o b l e m
67  */
68 SQProblemSchur::SQProblemSchur( ) : SQProblem( )
69 {
70 #ifdef SOLVER_MA57
71  sparseSolver = new Ma57SparseSolver();
72 #elif defined SOLVER_MA27
73  sparseSolver = new Ma27SparseSolver();
74 #elif defined SOLVER_NONE
75  sparseSolver = new DummySparseSolver();
76 #endif
77 
78  nSmax = 0;
79  nS = -1;
80  S = 0;
81  Q_ = 0;
82  R_ = 0;
83  detS = 0.0;
84  rcondS = 0.0;
85  schurUpdateIndex = 0;
86  schurUpdate = 0;
87  numFactorizations = 0;
88 
89  M_physicallength = 0;
90  M_vals = 0;
91  M_ir = 0;
92  M_jc = 0;
93 }
94 
95 
96 /*
97  * Q P r o b l e m
98  */
99 SQProblemSchur::SQProblemSchur( int_t _nV, int_t _nC, HessianType _hessianType, int_t maxSchurUpdates ) : SQProblem( _nV,_nC,_hessianType )
100 {
101  /* We use the variables Q and R to store the QR factorization of S.
102  * T is not required. */
103  delete [] R; R = 0;
104  delete [] Q; Q = 0;
105  delete [] T; T = 0;
106 
107  /* The interface to the sparse linear solver. In the long run,
108  different linear solvers might be optionally chosen. */
109 #ifdef SOLVER_MA57
110  sparseSolver = new Ma57SparseSolver();
111 #elif defined SOLVER_MA27
112  sparseSolver = new Ma27SparseSolver();
113 #elif defined SOLVER_NONE
114  sparseSolver = new DummySparseSolver();
115 #endif
116 
117  nSmax = maxSchurUpdates;
118  nS = -1;
119  if ( nSmax > 0 )
120  {
121  S = new real_t[nSmax*nSmax];
122  schurUpdateIndex = new int_t[nSmax];
123  schurUpdate = new SchurUpdateType[nSmax];
124  Q_ = new real_t[nSmax*nSmax];
125  R_ = new real_t[nSmax*nSmax];
126  M_physicallength = 10*nSmax; /* TODO: Decide good default. */
127  M_vals = new real_t[M_physicallength];
128  M_ir = new sparse_int_t[M_physicallength];
129  M_jc = new sparse_int_t[nSmax+1];
130  detS = 1.0;
131  rcondS = 1.0;
132  }
133  else
134  {
135  S = 0;
136  Q_ = 0;
137  R_ = 0;
138  detS = 0.0;
139  rcondS = 0.0;
140  schurUpdateIndex = 0;
141  schurUpdate = 0;
142  M_physicallength = 0;
143  M_vals = 0;
144  M_ir = 0;
145  M_jc = 0;
146  }
147  numFactorizations = 0;
148 }
149 
150 
151 /*
152  * Q P r o b l e m
153  */
154 SQProblemSchur::SQProblemSchur( const SQProblemSchur& rhs ) : SQProblem( rhs )
155 {
156 #ifdef SOLVER_MA57
157  sparseSolver = new Ma57SparseSolver();
158 #elif defined SOLVER_MA27
159  sparseSolver = new Ma27SparseSolver();
160 #elif defined SOLVER_NONE
161  sparseSolver = new DummySparseSolver();
162 #endif
163  copy( rhs );
164 }
165 
166 
167 /*
168  * ~ Q P r o b l e m
169  */
170 SQProblemSchur::~SQProblemSchur( )
171 {
172  delete sparseSolver;
173 
174  clear( );
175 }
176 
177 
178 /*
179  * o p e r a t o r =
180  */
181 SQProblemSchur& SQProblemSchur::operator=( const SQProblemSchur& rhs )
182 {
183  if ( this != &rhs )
184  {
185  clear( );
186  SQProblem::operator=( rhs );
187  copy( rhs );
188  }
189  return *this;
190 }
191 
192 
193 /*
194  * r e s e t
195  */
196 returnValue SQProblemSchur::reset( )
197 {
198  /* AW: We probably want to avoid resetting factorization in QProblem */
199  if ( SQProblem::reset( ) != SUCCESSFUL_RETURN )
200  return THROWERROR( RET_RESET_FAILED );
201 
202  sparseSolver->reset();
203  nS = -1;
204 
205  return SUCCESSFUL_RETURN;
206 }
207 
208 
209 /*****************************************************************************
210  * P R O T E C T E D *
211  *****************************************************************************/
212 
213 /*
214  * c l e a r
215  */
216 returnValue SQProblemSchur::clear( )
217 {
218  nSmax = 0;
219  nS = -1;
220  detS = 0.0;
221  rcondS = 0.0;
222  numFactorizations = 0;
223  delete [] S; S=0;
224  delete [] Q_; Q_=0;
225  delete [] R_; R_=0;
226  delete [] schurUpdateIndex; schurUpdateIndex=0;
227  delete [] schurUpdate; schurUpdate=0;
228  M_physicallength = 0;
229  delete [] M_vals; M_vals=0;
230  delete [] M_ir; M_ir=0;
231  delete [] M_jc; M_jc=0;
232 
233  return SUCCESSFUL_RETURN;
234 }
235 
236 
237 /*
238  * c o p y
239  */
240 returnValue SQProblemSchur::copy( const SQProblemSchur& rhs
241  )
242 {
243  int_t i, j, length;
244 
245  *sparseSolver = *(rhs.sparseSolver);
246 
247  nS = rhs.nS;
248  nSmax = rhs.nSmax;
249  if ( nSmax > 0 )
250  {
251  detS = rhs.detS;
252  rcondS = rhs.rcondS;
253  S = new real_t[nSmax*nSmax];
254  Q_ = new real_t[nSmax*nSmax];
255  R_ = new real_t[nSmax*nSmax];
256  schurUpdateIndex = new int_t[nSmax];
257  schurUpdate = new SchurUpdateType[nSmax];
258 
259  if ( nS>0 )
260  {
261  for ( i=0; i<nS; i++)
262  for ( j=0; j<nS; j++)
263  {
264  S[i*nSmax + j] = rhs.S[i*nSmax + j];
265  Q_[i*nSmax + j] = rhs.Q_[i*nSmax + j];
266  R_[i*nSmax + j] = rhs.R_[i*nSmax + j];
267  }
268 
269  memcpy( schurUpdateIndex, rhs.schurUpdateIndex, ((unsigned int)nS)*sizeof(int_t));
270  memcpy( schurUpdate, rhs.schurUpdate, ((unsigned int)nS)*sizeof(SchurUpdateType));
271  }
272 
273  M_physicallength = rhs.M_physicallength;
274  if ( M_physicallength>0 )
275  {
276  M_vals = new real_t[M_physicallength];
277  M_ir = new sparse_int_t[M_physicallength];
278  M_jc = new sparse_int_t[nSmax+1];
279 
280  if ( nS>0 )
281  {
282  memcpy(M_jc, rhs.M_jc, ((unsigned int)(nS+1))*sizeof(sparse_int_t));
283  length = M_jc[nS];
284  memcpy(M_vals, rhs.M_vals, ((unsigned int)length)*sizeof(real_t));
285  memcpy(M_ir, rhs.M_ir, ((unsigned int)length)*sizeof(sparse_int_t));
286  }
287  else if ( nS==0 )
288  M_jc[0] = rhs.M_jc[0];
289  }
290  }
291  else
292  {
293  S = 0;
294  Q_ = 0;
295  R_ = 0;
296  detS = 0.0;
297  rcondS = 0.0;
298  schurUpdateIndex = 0;
299  schurUpdate = 0;
300  M_physicallength = 0;
301  M_vals = 0;
302  M_ir = 0;
303  M_jc = 0;
304  }
305  numFactorizations = rhs.numFactorizations;
306 
307  boundsFreeStart = rhs.boundsFreeStart;
308  constraintsActiveStart = rhs.constraintsActiveStart;
309 
310  return SUCCESSFUL_RETURN;
311 }
312 
313 
314 /*
315  * s e t u p A u x i l i a r y Q P
316  */
317 returnValue SQProblemSchur::setupAuxiliaryQP( SymmetricMatrix *H_new,
318  Matrix *A_new,
319  const real_t *lb_new,
320  const real_t *ub_new,
321  const real_t *lbA_new,
322  const real_t *ubA_new
323  )
324 {
325  int_t i;
326  int_t nV = getNV( );
327  int_t nC = getNC( );
328  returnValue returnvalue;
329 
330  if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
331  ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
332  ( getStatus( ) == QPS_PERFORMINGHOMOTOPY ) )
333  {
334  return THROWERROR( RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED );
335  }
336 
337  status = QPS_PREPARINGAUXILIARYQP;
338 
339 
340  /* I) SETUP NEW QP MATRICES AND VECTORS: */
341  /* 1) Shift constraints' bounds vectors by (A_new - A)'*x_opt to ensure
342  * that old optimal solution remains feasible for new QP data. */
343  /* Firstly, shift by -A'*x_opt and ... */
344  if ( nC > 0 )
345  {
346  if ( A_new == 0 )
347  return THROWERROR( RET_INVALID_ARGUMENTS );
348 
349  for ( i=0; i<nC; ++i )
350  {
351  lbA[i] = -Ax_l[i];
352  ubA[i] = Ax_u[i];
353  }
354 
355  /* Set constraint matrix as well as ... */
356  setA( A_new );
357 
358  /* ... secondly, shift by +A_new'*x_opt. */
359  for ( i=0; i<nC; ++i )
360  {
361  lbA[i] += Ax[i];
362  ubA[i] += Ax[i];
363  }
364 
365  /* update constraint products. */
366  for ( i=0; i<nC; ++i )
367  {
368  Ax_u[i] = ubA[i] - Ax[i];
369  Ax_l[i] = Ax[i] - lbA[i];
370  }
371  }
372 
373  /* 2) Set new Hessian matrix,determine Hessian type and
374  * regularise new Hessian matrix if necessary. */
375  /* a) Setup new Hessian matrix and determine its type. */
376  if ( H_new != 0 )
377  {
378  setH( H_new );
379 
380  hessianType = HST_UNKNOWN;
381  if ( determineHessianType( ) != SUCCESSFUL_RETURN )
382  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
383 
384  /* b) Regularise new Hessian if necessary. */
385  if ( ( hessianType == HST_ZERO ) ||
386  ( hessianType == HST_SEMIDEF ) ||
387  ( usingRegularisation( ) == BT_TRUE ) )
388  {
389  regVal = 0.0; /* reset previous regularisation */
390 
391  if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
392  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
393  }
394  }
395  else
396  {
397  if ( H != 0 )
398  return THROWERROR( RET_NO_HESSIAN_SPECIFIED );
399  }
400 
401  /* 3) Setup QP gradient. */
402  if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
403  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
404 
405  /* II) SETUP WORKING SET AND MATRIX FACTORISATION: */
406 
407  /* 1) Check if current active set is linearly independent and has the correct inertia */
408  returnvalue = resetSchurComplement( BT_FALSE );
409  int_t neig = sparseSolver->getNegativeEigenvalues( );
410 
411  if ( returnvalue == SUCCESSFUL_RETURN && neig == getNAC( ) )
412  {
413  /* a) This means the proposed working set is linearly independent and
414  * leaves no zero curvature exposed in the nullspace and can be used to start QP solve. */
415  if ( options.printLevel == PL_HIGH )
416  MyPrintf( "In hotstart for new matrices, old working set is linearly independent and has correct inertia.\n");
417 
418  status = QPS_AUXILIARYQPSOLVED;
419  return SUCCESSFUL_RETURN;
420  }
421  else if ( returnvalue == SUCCESSFUL_RETURN && neig > getNAC( ) )
422  {
423  /* b) KKT matrix has too many negative eigenvalues. Try to correct the inertia by adding bounds (reduce nullspace dimension). */
424  if ( options.printLevel == PL_HIGH )
425  MyPrintf( "WARNING: In hotstart for new matrices, reduced Hessian for initial working set has %i negative eigenvalues, should be %i.\n", neig, getNAC( ) );
426 
427  /* If enabling inertia correction is disabled, exit here */
428  if ( options.enableInertiaCorrection )
429  {
430  returnvalue = correctInertia();
431  if ( returnvalue == SUCCESSFUL_RETURN )
432  {
433  status = QPS_AUXILIARYQPSOLVED;
434  return SUCCESSFUL_RETURN;
435  }
436  }
437  else
438  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
439  }
440 
441  /* 2) If inertia correction has failed or factorization yielded some other error,
442  * try to rebuild the active set with all simple bounds set according to initialStatusBounds
443  * (Note: in exact arithmetic, this cannot happen) */
444  if ( options.printLevel == PL_HIGH )
445  MyPrintf( "WARNING: hotstart for old active set failed. Trying to rebuild a working set.\n");
446 
447  Bounds oldBounds = bounds;
448  Constraints oldConstraints = constraints;
449 
450  /* Move all inactive variables to a bound */
451  for ( i=0; i<nV; i++ )
452  {
453  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
454  if ( bounds.getType( i ) == ST_EQUALITY )
455  {
456  if ( oldBounds.setStatus( i,ST_LOWER ) != SUCCESSFUL_RETURN )
457  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
458  }
459  else
460  #endif
461  {
462  if ( oldBounds.getStatus( i ) == ST_INACTIVE )
463  if ( oldBounds.setStatus( i, options.initialStatusBounds ) != SUCCESSFUL_RETURN )
464  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
465  }
466  }
467 
468  /* Set all equalities active */
469  #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
470  for( i=0; i<nC; ++i )
471  {
472  if ( constraints.getType( i ) == ST_EQUALITY )
473  if ( oldConstraints.setStatus( i,ST_LOWER ) != SUCCESSFUL_RETURN )
474  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
475  }
476  #endif
477 
478  /* Set all inequalities inactive */
479  for( i=0; i<nC; ++i )
480  {
481  if ( constraints.getType( i ) != ST_EQUALITY )
482  if ( oldConstraints.setStatus( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
483  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
484  }
485 
486  /* Reset bounds and constraints */
487  bounds.init( nV );
488  constraints.init( nC );
489 
490  if ( setupSubjectToType(lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
491  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
492 
493  if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
494  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
495 
496  if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
497  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
498 
499  /* Setup working sets afresh. */
500  if ( setupAuxiliaryWorkingSet( &oldBounds,&oldConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
501  return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
502 
503  /* adjust lb/ub */
504  for (int_t ii = 0; ii < nC; ++ii)
505  Ax_l[ii] = Ax_u[ii] = Ax[ii];
506  setupAuxiliaryQPbounds (&bounds, &constraints, BT_FALSE);
507 
508  status = QPS_AUXILIARYQPSOLVED;
509 
510  return SUCCESSFUL_RETURN;
511 }
512 
513 
514 /*
515  * s e t u p A u x i l i a r y W o r k i n g S e t
516  */
517 returnValue SQProblemSchur::setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds,
518  const Constraints* const auxiliaryConstraints,
519  BooleanType setupAfresh
520  )
521 {
522  int_t i;
523  int_t nV = getNV( );
524  int_t nC = getNC( );
525 
526  /* consistency checks */
527  if ( auxiliaryBounds != 0 )
528  {
529  for( i=0; i<nV; ++i )
530  if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
531  return THROWERROR( RET_UNKNOWN_BUG );
532  }
533  else
534  {
535  return THROWERROR( RET_INVALID_ARGUMENTS );
536  }
537 
538  if ( auxiliaryConstraints != 0 )
539  {
540  for( i=0; i<nC; ++i )
541  if ( ( constraints.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryConstraints->getStatus( i ) == ST_UNDEFINED ) )
542  return THROWERROR( RET_UNKNOWN_BUG );
543  }
544  else
545  {
546  return THROWERROR( RET_INVALID_ARGUMENTS );
547  }
548 
549  /* I.) REMOVE INEQUALITY BOUNDS/CONSTRAINTS */
550 
551  /* I.1) Remove inequality bounds that are active now but shall be
552  * inactive or active at the other bound according to auxiliaryBounds */
553  for( i=0; i<nV; ++i )
554  {
555  if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
556  if ( bounds.moveFixedToFree( i ) != SUCCESSFUL_RETURN )
557  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
558 
559  if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
560  if ( bounds.moveFixedToFree( i ) != SUCCESSFUL_RETURN )
561  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
562  }
563 
564  /* I.2.) Remove inequality constraints that are active now but shall be
565  * inactive or active at the other bound according to auxiliaryConstraints */
566  for( i=0; i<nC; ++i )
567  {
568  if ( ( constraints.getStatus( i ) == ST_LOWER ) && ( auxiliaryConstraints->getStatus( i ) != ST_LOWER ) )
569  if ( constraints.moveActiveToInactive( i ) != SUCCESSFUL_RETURN )
570  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
571 
572  if ( ( constraints.getStatus( i ) == ST_UPPER ) && ( auxiliaryConstraints->getStatus( i ) != ST_UPPER ) )
573  if ( constraints.moveActiveToInactive( i ) != SUCCESSFUL_RETURN )
574  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
575  }
576 
577  /* II.) ADD BOUNDS/CONSTRAINTS */
578 
579  /* II.1.) Add bounds according to auxiliaryBounds */
580  for( i=0; i<nV; ++i )
581  {
582  if ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) )
583  if ( bounds.moveFreeToFixed( i, auxiliaryBounds->getStatus( i ) ) != SUCCESSFUL_RETURN )
584  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
585  }
586 
587  /* II.2.) Add constraints according to auxiliaryConstraints */
588  for( i=0; i<nC; ++i )
589  {
590  if ( ( constraints.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryConstraints->getStatus( i ) != ST_INACTIVE ) )
591  if ( constraints.moveInactiveToActive( i,auxiliaryConstraints->getStatus( i ) ) != SUCCESSFUL_RETURN )
592  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
593  }
594 
595  /* III) FACTORIZATION */
596 
597  /* III.1.) Factorize (resolves linear dependency) */
598  if( resetSchurComplement( BT_FALSE ) != SUCCESSFUL_RETURN )
599  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
600 
601  /* III.2.) Check if inertia is correct. If so, we now have a linearly independent working set with a pos def reduced Hessian */
602  int_t neig = sparseSolver->getNegativeEigenvalues( );
603  if ( neig == getNAC( ) )
604  {
605  /* We now have a linearly independent working set with a pos def reduced Hessian.
606  * We need to correct the QP bounds and gradient after this. */
607  return SUCCESSFUL_RETURN;
608  }
609 
610  /* IV.) INERTIA CORRECTION IF NECESSARY */
611 
612  /* We now have a fresh factorization and can start the usual inertia correction routine */
613  if ( options.printLevel == PL_HIGH )
614  MyPrintf( "WARNING: In setupAuxiliaryWorkingSet: Initial working set reduced Hessian has %i negative eigenvalues, should be %i.\n", neig, getNAC( ) );
615 
616  if ( options.enableInertiaCorrection == BT_TRUE )
617  return correctInertia( );
618  else
619  return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
620 }
621 
622 
623 /*
624  * 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
625  */
626 returnValue SQProblemSchur::computeProjectedCholesky( )
627 {
628  return SUCCESSFUL_RETURN;
629 }
630 
631 
632 /*
633  * c o m p u t e I n i t i a l C h o l e s k y
634  */
635 returnValue SQProblemSchur::computeInitialCholesky( )
636 {
637  return SUCCESSFUL_RETURN;
638 }
639 
640 
641 /*
642  * s e t u p T Q f a c t o r i s a t i o n
643  */
644 returnValue SQProblemSchur::setupTQfactorisation( )
645 {
646  return SUCCESSFUL_RETURN;
647 }
648 
649 
650 /*
651  * a d d C o n s t r a i n t
652  */
653 returnValue SQProblemSchur::addConstraint( int_t number,
654  SubjectToStatus C_status,
655  BooleanType updateCholesky,
656  BooleanType ensureLI
657  )
658 {
659  int_t idxDeleted = -1;
660 
661  /* consistency checks */
662  if ( constraints.getStatus( number ) != ST_INACTIVE )
663  return THROWERROR( RET_CONSTRAINT_ALREADY_ACTIVE );
664 
665  if ( ( constraints.getNC( ) - getNAC( ) ) == constraints.getNUC( ) )
666  return THROWERROR( RET_ALL_CONSTRAINTS_ACTIVE );
667 
668  if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
669  ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
670  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
671  ( getStatus( ) == QPS_SOLVED ) )
672  {
673  return THROWERROR( RET_UNKNOWN_BUG );
674  }
675 
676 
677  /* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
678  * i.e. remove a constraint or bound if linear dependence occurs. */
679  if ( ensureLI == BT_TRUE )
680  {
681  returnValue ensureLIreturnvalue = addConstraint_ensureLI( number,C_status );
682 
683  switch ( ensureLIreturnvalue )
684  {
685  case SUCCESSFUL_RETURN:
686  break;
687 
688  case RET_LI_RESOLVED:
689  break;
690 
691  case RET_ENSURELI_FAILED_NOINDEX:
692  return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;
693 
694  case RET_ENSURELI_FAILED_CYCLING:
695  return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;
696 
697  case RET_ENSURELI_DROPPED:
698  return SUCCESSFUL_RETURN;
699 
700  default:
701  return THROWERROR( RET_ENSURELI_FAILED );
702  }
703  }
704 
705  /* IV) UPDATE INDICES */
706  tabularOutput.idxAddC = number;
707  if ( constraints.moveInactiveToActive( number,C_status ) != SUCCESSFUL_RETURN )
708  return THROWERROR( RET_ADDCONSTRAINT_FAILED );
709 
710  /* Also update the Schur complement. */
711 
712  /* First check if this constraint had been removed before. In that
713  case delete this constraint from the Schur complement. */
714  bool found = false;
715  for ( int_t i=0; i<nS; i++ )
716  {
717  if ( schurUpdate[i] == SUT_ConRemoved && number == schurUpdateIndex[i] )
718  {
719  if ( deleteFromSchurComplement( i ) != SUCCESSFUL_RETURN )
720  return THROWERROR( RET_ADDCONSTRAINT_FAILED );
721  found = true;
722  idxDeleted = i;
723  break;
724  }
725  }
726  if ( !found )
727  {
728  if ( nS < 0 || nS==nSmax )
729  {
730  /* The schur complement has become too large, reset. */
731  /* Correct inertia if necessary. */
732  returnValue retval = resetSchurComplement( BT_TRUE );
733  if ( retval != SUCCESSFUL_RETURN )
734  {
735  if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
736  MyPrintf( "In addConstraint: KKT matrix singular when resetting Schur complement\n" );
737  else if ( options.printLevel == PL_HIGH )
738  MyPrintf( "In addConstraint, resetSchurComplement failed with retval = %d\n", retval);
739  return THROWERROR( RET_ADDCONSTRAINT_FAILED );
740  }
741  found = true;
742  }
743  else
744  {
745  /* If the constraint was not yet in Schur complement, add it now. */
746  int_t nFRStart = boundsFreeStart.getLength();
747  int_t* FR_idxStart;
748  boundsFreeStart.getNumberArray( &FR_idxStart );
749 
750  sparse_int_t* MNpos = new sparse_int_t[nFRStart+nS]; // This is an overestimate
751  real_t* MNvals = new real_t[nFRStart+nS];
752 
753  int_t* irn = new int_t[nFRStart+nS];
754  int_t* jcn = new int_t[nFRStart+nS];
755  real_t* vals = new real_t[nFRStart+nS];
756  int_t* icolsNumber = new int_t[nFRStart+nS];
757  int_t* icolsSIdx = new int_t[nS];
758 
759  for ( int_t i=0; i<nFRStart; i++)
760  icolsNumber[i] = FR_idxStart[i];
761 
762  int_t icolsLength = nFRStart;
763  for ( int_t i=0; i<nS; i++)
764  if ( schurUpdate[i] == SUT_VarFreed )
765  {
766  icolsNumber[icolsLength] = schurUpdateIndex[i];
767  icolsSIdx[icolsLength-nFRStart] = i;
768  icolsLength++;
769  }
770 
771  if ( constraintProduct != 0 )
772  {
773  MyPrintf( "In SQProblemSchur::addConstraint, constraintProduct not yet implemented.\n");
774  return THROWERROR(RET_NOT_YET_IMPLEMENTED);
775  }
776  int_t numNonzerosA;
777  A->getSparseSubmatrix( 1, &number, icolsLength, icolsNumber, 0, 0, numNonzerosA, irn, jcn, vals );
778  delete [] irn;
779 
780  int_t numNonzerosM = 0;
781  int_t numNonzerosN = 0;
782  for ( int_t i=0; i<numNonzerosA; i++ )
783  if ( jcn[i] < nFRStart )
784  {
785  MNpos[numNonzerosM] = jcn[i];
786  MNvals[numNonzerosM] = vals[i];
787  numNonzerosM++;
788  }
789  else
790  {
791  MNpos[nFRStart+numNonzerosN] = icolsSIdx[jcn[i]-nFRStart];
792  MNvals[nFRStart+numNonzerosN] = vals[i];
793  numNonzerosN++;
794  }
795 
796  returnValue returnvalue = addToSchurComplement( number, SUT_ConAdded, numNonzerosM, MNpos, MNvals, numNonzerosN, MNpos+nFRStart, MNvals+nFRStart, 0.0 );
797 
798  delete [] icolsSIdx;
799  delete [] icolsNumber;
800  delete [] vals;
801  delete [] jcn;
802  delete [] MNvals;
803  delete [] MNpos;
804 
805  if ( returnvalue != SUCCESSFUL_RETURN )
806  return THROWERROR( RET_ADDCONSTRAINT_FAILED );
807 
808  found = true;
809  }
810  }
811 
812  if ( !found )
813  return THROWERROR( RET_ADDCONSTRAINT_FAILED );
814 
815  updateSchurQR( idxDeleted );
816 
817  /* If reciprocal of condition number becomes to small, refactorize KKT matrix */
818  if( rcondS < options.rcondSMin )
819  {
820  returnValue retval = resetSchurComplement( BT_TRUE );
821  if ( retval != SUCCESSFUL_RETURN )
822  {
823  if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
824  MyPrintf( "In addConstraint: KKT matrix singular when resetting Schur complement\n" );
825  else if ( options.printLevel == PL_HIGH )
826  MyPrintf( "In addConstraint, resetSchurComplement failed with retval = %d\n", retval);
827  return THROWERROR( RET_ADDCONSTRAINT_FAILED );
828  }
829  }
830 
831  return SUCCESSFUL_RETURN;
832 }
833 
834 
835 
836 /*
837  * a d d C o n s t r a i n t _ c h e c k L I
838  */
839 returnValue SQProblemSchur::addConstraint_checkLI( int_t number )
840 {
841  /* Get space for the multipliers xi in linear independence test */
842  int_t nAC = getNAC();
843  int_t nFX = getNFX();
844  real_t *xiC = new real_t[nAC];
845  real_t *xiB = new real_t[nFX];
846 
847  /* I) Check if new constraint is linearly independent from the active ones. */
848  returnValue returnvalueCheckLI = addConstraint_checkLISchur( number, xiC, xiB );
849 
850  delete [] xiB;
851  delete [] xiC;
852 
853  return returnvalueCheckLI;
854 }
855 
856 
857 /*
858  * a d d C o n s t r a i n t _ c h e c k L I S c h u r
859  */
860 returnValue SQProblemSchur::addConstraint_checkLISchur( int_t number, real_t* xiC, real_t* xiB )
861 {
862  returnValue returnvalue = RET_LINEARLY_DEPENDENT;
863 
864  int_t ii;
865  int_t nV = getNV( );
866  int_t nFR = getNFR( );
867  int_t nC = getNC( );
868  int_t nAC = getNAC();
869  int_t nFX = getNFX();
870  int_t *FR_idx;
871 
872  bounds.getFree( )->getNumberArray( &FR_idx );
873 
874  /* For the Schur complement version we only use options.enableFullLITests = TRUE */
875  {
876  /*
877  * expensive LI test. Backsolve with refinement using special right
878  * hand side. This gives an estimate for what should be considered
879  * "zero". We then check linear independence relative to this estimate.
880  */
881 
882  int_t *FX_idx, *AC_idx, *IAC_idx;
883 
884  real_t *delta_g = new real_t[nV];
885  real_t *delta_xFX = new real_t[nFX];
886  real_t *delta_xFR = new real_t[nFR];
887  real_t *delta_yAC = xiC;
888  real_t *delta_yFX = xiB;
889 
890  bounds.getFixed( )->getNumberArray( &FX_idx );
891  constraints.getActive( )->getNumberArray( &AC_idx );
892  constraints.getInactive( )->getNumberArray( &IAC_idx );
893 
894  int_t dim = (nC>nV)?nC:nV;
895  real_t *nul = new real_t[dim];
896  for (ii = 0; ii < dim; ++ii)
897  nul[ii]=0.0;
898 
899  A->getRow (number, 0, 1.0, delta_g);
900 
901  returnValue dsdreturnvalue = determineStepDirection ( delta_g,
902  nul, nul, nul, nul,
903  BT_FALSE, BT_FALSE,
904  delta_xFX, delta_xFR, delta_yAC, delta_yFX);
905  if (dsdreturnvalue!=SUCCESSFUL_RETURN)
906  returnvalue = dsdreturnvalue;
907 
908  delete[] nul;
909 
910  /* compute the weight in inf-norm */
911  real_t weight = 0.0;
912  for (ii = 0; ii < nAC; ++ii)
913  {
914  real_t a = getAbs (delta_yAC[ii]);
915  if (weight < a) weight = a;
916  }
917  for (ii = 0; ii < nFX; ++ii)
918  {
919  real_t a = getAbs (delta_yFX[ii]);
920  if (weight < a) weight = a;
921  }
922 
923  /* look at the "zero" in a relative inf-norm */
924  real_t zero = 0.0;
925  for (ii = 0; ii < nFX; ++ii)
926  {
927  real_t a = getAbs (delta_xFX[ii]);
928  if (zero < a) zero = a;
929  }
930  for (ii = 0; ii < nFR; ++ii)
931  {
932  real_t a = getAbs (delta_xFR[ii]);
933  if (zero < a) zero = a;
934  }
935 
936  /* relative test against zero in inf-norm */
937  if (zero > options.epsLITests * weight)
938  returnvalue = RET_LINEARLY_INDEPENDENT;
939 
940  delete[] delta_xFR;
941  delete[] delta_xFX;
942  delete[] delta_g;
943 
944  }
945  return THROWINFO( returnvalue );
946 }
947 
948 
949 /*
950  * a d d C o n s t r a i n t _ e n s u r e L I
951  */
952 returnValue SQProblemSchur::addConstraint_ensureLI( int_t number, SubjectToStatus C_status )
953 {
954  /* Get space for the multipliers xi in linear independence test */
955  int_t nAC = getNAC();
956  int_t nFX = getNFX();
957  real_t *xiC = new real_t[nAC];
958  real_t *xiB = new real_t[nFX];
959 
960  /* I) Check if new constraint is linearly independent from the active ones. */
961  returnValue returnvalueCheckLI = addConstraint_checkLISchur( number, xiC, xiB );
962 
963  if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
964  {
965  delete [] xiB;
966  delete [] xiC;
967  return THROWERROR( RET_ENSURELI_FAILED );
968  }
969 
970  if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
971  {
972  delete [] xiB;
973  delete [] xiC;
974  return SUCCESSFUL_RETURN;
975  }
976 
977  /* II) NEW BOUND IS LINEARLY DEPENDENT: */
978  /* 1) Coefficients of linear combination, have already been computed, but we need to correct the sign. */
979  int_t i, ii;
980 
981  if ( C_status != ST_LOWER )
982  {
983  for( i=0; i<nAC; ++i )
984  xiC[i] = -xiC[i];
985  for( i=0; i<nFX; ++i )
986  xiB[i] = -xiB[i];
987  }
988 
989  int_t nV = getNV( );
990 
991  int_t* FX_idx;
992  bounds.getFixed( )->getNumberArray( &FX_idx );
993 
994  int_t* AC_idx;
995  constraints.getActive( )->getNumberArray( &AC_idx );
996 
997  real_t* num = new real_t[nV];
998 
999  real_t y_min = options.maxDualJump;
1000  int_t y_min_number = -1;
1001  int_t y_min_number_bound = -1;
1002  BooleanType y_min_isBound = BT_FALSE;
1003 
1004  returnValue returnvalue = SUCCESSFUL_RETURN;
1005 
1006  /* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
1007 
1008  /* 1) Constraints. */
1009  for( i=0; i<nAC; ++i )
1010  {
1011  ii = AC_idx[i];
1012  num[i] = y[nV+ii];
1013  }
1014 
1015  performRatioTest (nAC, AC_idx, &constraints, num, xiC, options.epsNum, options.epsDen, y_min, y_min_number);
1016 
1017  /* 2) Bounds. */
1018  for( i=0; i<nFX; ++i )
1019  {
1020  ii = FX_idx[i];
1021  num[i] = y[ii];
1022  }
1023 
1024  performRatioTest (nFX, FX_idx, &bounds, num, xiB, options.epsNum, options.epsDen, y_min, y_min_number_bound);
1025 
1026  if ( y_min_number_bound >= 0 )
1027  {
1028  y_min_number = y_min_number_bound;
1029  y_min_isBound = BT_TRUE;
1030  }
1031 
1032  #ifndef __XPCTARGET__
1033  /* setup output preferences */
1034  char messageString[80];
1035  #endif
1036 
1037  /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
1038  if ( y_min_number >= 0 )
1039  {
1040  /* Update Lagrange multiplier... */
1041  for( i=0; i<nAC; ++i )
1042  {
1043  ii = AC_idx[i];
1044  y[nV+ii] -= y_min * xiC[i];
1045  }
1046  for( i=0; i<nFX; ++i )
1047  {
1048  ii = FX_idx[i];
1049  y[ii] -= y_min * xiB[i];
1050  }
1051 
1052  /* ... also for newly active constraint... */
1053  if ( C_status == ST_LOWER )
1054  y[nV+number] = y_min;
1055  else
1056  y[nV+number] = -y_min;
1057 
1058  /* ... and for constraint to be removed. */
1059  if ( y_min_isBound == BT_TRUE )
1060  {
1061  #ifndef __XPCTARGET__
1062  snprintf( messageString,80,"bound no. %d.",(int)y_min_number );
1063  getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
1064  #endif
1065 
1066  if ( removeBound( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
1067  {
1068  returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
1069  goto farewell;
1070  }
1071  tabularOutput.excRemB = 1;
1072 
1073  y[y_min_number] = 0.0;
1074  }
1075  else
1076  {
1077  #ifndef __XPCTARGET__
1078  snprintf( messageString,80,"constraint no. %d.",(int)y_min_number );
1079  getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
1080  #endif
1081 
1082  if ( removeConstraint( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
1083  {
1084  returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
1085  goto farewell;
1086  }
1087  tabularOutput.excRemC = 1;
1088 
1089  y[nV+y_min_number] = 0.0;
1090  }
1091  }
1092  else
1093  {
1094  if (options.enableDropInfeasibles == BT_TRUE) {
1095  /* dropping of infeasible constraints according to drop priorities */
1096  returnvalue = dropInfeasibles (number, C_status, BT_FALSE, xiB, xiC);
1097  }
1098  else
1099  {
1100  /* no constraint/bound can be removed => QP is infeasible! */
1101  returnvalue = RET_ENSURELI_FAILED_NOINDEX;
1102  setInfeasibilityFlag( returnvalue );
1103  }
1104  }
1105 
1106 farewell:
1107  delete[] num;
1108  delete [] xiB;
1109  delete [] xiC;
1110 
1111  getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
1112 
1113  return (returnvalue != SUCCESSFUL_RETURN) ? THROWERROR (returnvalue) : returnvalue;
1114 }
1115 
1116 
1117 /*
1118  * a d d B o u n d
1119  */
1120 returnValue SQProblemSchur::addBound( int_t number,
1121  SubjectToStatus B_status,
1122  BooleanType updateCholesky,
1123  BooleanType ensureLI
1124  )
1125 {
1126  int_t idxDeleted = -1;
1127 
1128  /* consistency checks */
1129  if ( bounds.getStatus( number ) != ST_INACTIVE )
1130  return THROWERROR( RET_BOUND_ALREADY_ACTIVE );
1131 
1132  if ( getNFR( ) == bounds.getNUV( ) )
1133  return THROWERROR( RET_ALL_BOUNDS_ACTIVE );
1134 
1135  if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
1136  ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
1137  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
1138  ( getStatus( ) == QPS_SOLVED ) )
1139  {
1140  return THROWERROR( RET_UNKNOWN_BUG );
1141  }
1142 
1143 
1144  /* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
1145  * i.e. remove a constraint or bound if linear dependence occurs. */
1146  if ( ensureLI == BT_TRUE )
1147  {
1148  returnValue ensureLIreturnvalue = addBound_ensureLI( number,B_status );
1149 
1150  switch ( ensureLIreturnvalue )
1151  {
1152  case SUCCESSFUL_RETURN:
1153  break;
1154 
1155  case RET_LI_RESOLVED:
1156  break;
1157 
1158  case RET_ENSURELI_FAILED_NOINDEX:
1159  return RET_ADDBOUND_FAILED_INFEASIBILITY;
1160 
1161  case RET_ENSURELI_FAILED_CYCLING:
1162  return RET_ADDBOUND_FAILED_INFEASIBILITY;
1163 
1164  case RET_ENSURELI_DROPPED:
1165  return SUCCESSFUL_RETURN;
1166 
1167  default:
1168  return THROWERROR( RET_ENSURELI_FAILED );
1169  }
1170  }
1171 
1172  /* II) UPDATE INDICES */
1173  tabularOutput.idxAddB = number;
1174  if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
1175  return THROWERROR( RET_ADDBOUND_FAILED );
1176 
1177  /* Also update the Schur complement. */
1178 
1179  /* First check if this variable had been freed before. In that
1180  case delete this variable from the Schur complement. */
1181  bool found = false;
1182  for ( int_t i=0; i<nS; i++ )
1183  {
1184  if ( schurUpdate[i] == SUT_VarFreed && number == schurUpdateIndex[i] )
1185  {
1186  if ( deleteFromSchurComplement( i ) != SUCCESSFUL_RETURN )
1187  return THROWERROR( RET_ADDBOUND_FAILED );
1188  found = true;
1189  idxDeleted = i;
1190  break;
1191  }
1192  }
1193  if ( !found )
1194  {
1195  if ( nS < 0 || nS==nSmax )
1196  {
1197  /* The schur complement has become too large, reset. */
1198  /* Correct inertia if necessary. */
1199  returnValue retval = resetSchurComplement( BT_TRUE );
1200  if ( retval != SUCCESSFUL_RETURN )
1201  {
1202  if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
1203  MyPrintf( "In addBound: KKT matrix singular when resetting Schur complement\n" );
1204  else if ( options.printLevel == PL_HIGH )
1205  MyPrintf( "In addBound, resetSchurComplement failed with retval = %d\n", retval);
1206  return THROWERROR( RET_ADDBOUND_FAILED );
1207  }
1208  found = true;
1209  }
1210  else
1211  {
1212  /* If the variable was not yet in Schur complement, add it now. */
1213  int_t nFRStart = boundsFreeStart.getLength();
1214  int_t* FR_idxStart;
1215  boundsFreeStart.getNumberArray( &FR_idxStart );
1216  for ( int_t i=0; i<nFRStart; i++ )
1217  if ( FR_idxStart[i] == number )
1218  {
1219  real_t one = 1.0;
1220  sparse_int_t pos = i;
1221  if ( addToSchurComplement( number, SUT_VarFixed, 1, &pos, &one, 0, 0, 0, 0.0 ) != SUCCESSFUL_RETURN )
1222  return THROWERROR( RET_ADDBOUND_FAILED );
1223  found = true;
1224  break;
1225  }
1226  }
1227  }
1228 
1229  if ( !found )
1230  return THROWERROR( RET_ADDBOUND_FAILED );
1231 
1232  updateSchurQR( idxDeleted );
1233 
1234  /* If reciprocal of condition number becomes to small, refactorize KKT matrix */
1235  if( rcondS < options.rcondSMin )
1236  {
1237  returnValue retval = resetSchurComplement( BT_TRUE );
1238  if ( retval != SUCCESSFUL_RETURN )
1239  {
1240  if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
1241  MyPrintf( "In addBound: KKT matrix singular when resetting Schur complement\n" );
1242  else if ( options.printLevel == PL_HIGH )
1243  MyPrintf( "In addBound, resetSchurComplement failed with retval = %d\n", retval);
1244  return THROWERROR( RET_ADDCONSTRAINT_FAILED );
1245  }
1246  }
1247 
1248  return SUCCESSFUL_RETURN;
1249 }
1250 
1251 
1252 /*
1253  * a d d B o u n d _ c h e c k L I
1254  */
1255 returnValue SQProblemSchur::addBound_checkLI( int_t number )
1256 {
1257  /* Get space for the multipliers xi in linear independence test */
1258  int_t nAC = getNAC();
1259  int_t nFX = getNFX();
1260  real_t *xiC = new real_t[nAC];
1261  real_t *xiB = new real_t[nFX];
1262 
1263  /* I) Check if new constraint is linearly independent from the active ones. */
1264  returnValue returnvalueCheckLI = addBound_checkLISchur( number, xiC, xiB );
1265 
1266  delete [] xiB;
1267  delete [] xiC;
1268 
1269  return returnvalueCheckLI;
1270 }
1271 
1272 /*
1273  * a d d B o u n d _ c h e c k L I S c h u r
1274  */
1275 returnValue SQProblemSchur::addBound_checkLISchur( int_t number, real_t* xiC, real_t* xiB )
1276 {
1277  returnValue returnvalue = RET_LINEARLY_DEPENDENT;
1278 
1279 
1280  int_t ii;
1281  int_t nV = getNV( );
1282  int_t nFR = getNFR( );
1283  int_t nC = getNC( );
1284  int_t nAC = getNAC();
1285  int_t nFX = getNFX();
1286  int_t *FR_idx;
1287 
1288  bounds.getFree( )->getNumberArray( &FR_idx );
1289 
1290  /* For the Schur complement version we only use options.enableFullLITests = TRUE */
1291  {
1292  /*
1293  * expensive LI test. Backsolve with refinement using special right
1294  * hand side. This gives an estimate for what should be considered
1295  * "zero". We then check linear independence relative to this estimate.
1296  */
1297 
1298  real_t *delta_g = new real_t[nV];
1299  real_t *delta_xFX = new real_t[nFX];
1300  real_t *delta_xFR = new real_t[nFR];
1301  real_t *delta_yAC = xiC;
1302  real_t *delta_yFX = xiB;
1303 
1304  for (ii = 0; ii < nV; ++ii)
1305  delta_g[ii] = 0.0;
1306  delta_g[number] = 1.0;
1307 
1308  int_t dim = (nC>nV)?nC:nV;
1309  real_t *nul = new real_t[dim];
1310  for (ii = 0; ii < dim; ++ii)
1311  nul[ii]=0.0;
1312 
1313  returnValue dsdReturnValue = determineStepDirection (
1314  delta_g, nul, nul, nul, nul, BT_FALSE, BT_FALSE,
1315  delta_xFX, delta_xFR, delta_yAC, delta_yFX);
1316  if (dsdReturnValue != SUCCESSFUL_RETURN)
1317  returnvalue = dsdReturnValue;
1318 
1319  /* compute the weight in inf-norm */
1320  real_t weight = 0.0;
1321  for (ii = 0; ii < nAC; ++ii)
1322  {
1323  real_t a = getAbs (delta_yAC[ii]);
1324  if (weight < a) weight = a;
1325  }
1326  for (ii = 0; ii < nFX; ++ii)
1327  {
1328  real_t a = getAbs (delta_yFX[ii]);
1329  if (weight < a) weight = a;
1330  }
1331 
1332  /* look at the "zero" in a relative inf-norm */
1333  real_t zero = 0.0;
1334  for (ii = 0; ii < nFX; ++ii)
1335  {
1336  real_t a = getAbs (delta_xFX[ii]);
1337  if (zero < a) zero = a;
1338  }
1339  for (ii = 0; ii < nFR; ++ii)
1340  {
1341  real_t a = getAbs (delta_xFR[ii]);
1342  if (zero < a) zero = a;
1343  }
1344 
1345  /* relative test against zero in inf-norm */
1346  if (zero > options.epsLITests * weight)
1347  returnvalue = RET_LINEARLY_INDEPENDENT;
1348 
1349  delete[] nul;
1350  delete[] delta_xFR;
1351  delete[] delta_xFX;
1352  delete[] delta_g;
1353 
1354  }
1355  return THROWINFO( returnvalue );
1356 }
1357 
1358 
1359 /*
1360  * a d d B o u n d _ e n s u r e L I
1361  */
1362 returnValue SQProblemSchur::addBound_ensureLI( int_t number, SubjectToStatus B_status )
1363 {
1364  /* Get space for the multipliers xi in linear independence test */
1365  int_t nAC = getNAC();
1366  int_t nFX = getNFX();
1367  real_t *xiC = new real_t[nAC];
1368  real_t *xiB = new real_t[nFX];
1369 
1370  /* I) Check if new constraint is linearly independent from the active ones. */
1371  returnValue returnvalueCheckLI = addBound_checkLISchur( number, xiC, xiB );
1372 
1373  if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
1374  {
1375  delete [] xiB;
1376  delete [] xiC;
1377  return THROWERROR( RET_ENSURELI_FAILED );
1378  }
1379 
1380  if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
1381  {
1382  delete [] xiB;
1383  delete [] xiC;
1384  return SUCCESSFUL_RETURN;
1385  }
1386 
1387  /* II) NEW BOUND IS LINEARLY DEPENDENT: */
1388  /* 1) Coefficients of linear combination, have already been computed, but we need to correct the sign. */
1389  int_t i, ii;
1390 
1391  if ( B_status != ST_LOWER )
1392  {
1393  for( i=0; i<nAC; ++i )
1394  xiC[i] = -xiC[i];
1395  for( i=0; i<nFX; ++i )
1396  xiB[i] = -xiB[i];
1397  }
1398 
1399  int_t nV = getNV( );
1400 
1401  int_t* FX_idx;
1402  bounds.getFixed( )->getNumberArray( &FX_idx );
1403 
1404  int_t* AC_idx;
1405  constraints.getActive( )->getNumberArray( &AC_idx );
1406 
1407  real_t* num = new real_t[nV];
1408 
1409  real_t y_min = options.maxDualJump;
1410  int_t y_min_number = -1;
1411  int_t y_min_number_bound = -1;
1412  BooleanType y_min_isBound = BT_FALSE;
1413 
1414  returnValue returnvalue = SUCCESSFUL_RETURN;
1415 
1416  /* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
1417 
1418  /* 1) Constraints. */
1419  for( i=0; i<nAC; ++i )
1420  {
1421  ii = AC_idx[i];
1422  num[i] = y[nV+ii];
1423  }
1424 
1425  performRatioTest( nAC,AC_idx,&constraints, num,xiC, options.epsNum, options.epsDen, y_min,y_min_number );
1426 
1427  /* 2) Bounds. */
1428  for( i=0; i<nFX; ++i )
1429  {
1430  ii = FX_idx[i];
1431  num[i] = y[ii];
1432  }
1433 
1434  performRatioTest( nFX,FX_idx,&bounds, num,xiB, options.epsNum, options.epsDen, y_min,y_min_number_bound );
1435 
1436  if ( y_min_number_bound >= 0 )
1437  {
1438  y_min_number = y_min_number_bound;
1439  y_min_isBound = BT_TRUE;
1440  }
1441 
1442  /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
1443  char messageString[80];
1444 
1445  if ( y_min_number >= 0 )
1446  {
1447  /* Update Lagrange multiplier... */
1448  for( i=0; i<nAC; ++i )
1449  {
1450  ii = AC_idx[i];
1451  y[nV+ii] -= y_min * xiC[i];
1452  }
1453  for( i=0; i<nFX; ++i )
1454  {
1455  ii = FX_idx[i];
1456  y[ii] -= y_min * xiB[i];
1457  }
1458 
1459  /* ... also for newly active bound ... */
1460  if ( B_status == ST_LOWER )
1461  y[number] = y_min;
1462  else
1463  y[number] = -y_min;
1464 
1465  /* ... and for bound to be removed. */
1466  if ( y_min_isBound == BT_TRUE )
1467  {
1468  #ifndef __XPCTARGET__
1469  snprintf( messageString,80,"bound no. %d.",(int)y_min_number );
1470  getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
1471  #endif
1472 
1473  if ( removeBound( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
1474  {
1475  returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
1476  goto farewell;
1477  }
1478  tabularOutput.excRemB = 1;
1479 
1480  y[y_min_number] = 0.0;
1481  }
1482  else
1483  {
1484  #ifndef __XPCTARGET__
1485  snprintf( messageString,80,"constraint no. %d.",(int)y_min_number );
1486  getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
1487  #endif
1488 
1489  if ( removeConstraint( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
1490  {
1491  returnvalue = RET_REMOVE_FROM_ACTIVESET_FAILED;
1492  goto farewell;
1493  }
1494  tabularOutput.excRemC = 1;
1495 
1496  y[nV+y_min_number] = 0.0;
1497  }
1498  }
1499  else
1500  {
1501  if (options.enableDropInfeasibles == BT_TRUE) {
1502  /* dropping of infeasible constraints according to drop priorities */
1503  returnvalue = dropInfeasibles (number, B_status, BT_TRUE, xiB, xiC);
1504  }
1505  else
1506  {
1507  /* no constraint/bound can be removed => QP is infeasible! */
1508  returnvalue = RET_ENSURELI_FAILED_NOINDEX;
1509  setInfeasibilityFlag( returnvalue );
1510  }
1511  }
1512 
1513 farewell:
1514  delete[] num;
1515  delete[] xiB;
1516  delete[] xiC;
1517 
1518  getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
1519 
1520  return (returnvalue != SUCCESSFUL_RETURN) ? THROWERROR (returnvalue) : returnvalue;
1521 }
1522 
1523 
1524 
1525 /*
1526  * r e m o v e C o n s t r a i n t
1527  */
1528 returnValue SQProblemSchur::removeConstraint( int_t number,
1529  BooleanType updateCholesky,
1530  BooleanType allowFlipping,
1531  BooleanType ensureNZC
1532  )
1533 {
1534  returnValue returnvalue = SUCCESSFUL_RETURN;
1535 
1536  int_t sModType = 0;
1537  int_t idxDeleted = -1;
1538  SubjectToStatus oldStatus;
1539  real_t oldDet, newDet;
1540 
1541  /* consistency check */
1542  if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
1543  ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
1544  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
1545  ( getStatus( ) == QPS_SOLVED ) )
1546  {
1547  return THROWERROR( RET_UNKNOWN_BUG );
1548  }
1549 
1550  /* some definitions */
1551  int_t nAC = getNAC( );
1552  int_t number_idx = constraints.getActive( )->getIndex( number );
1553 
1554  int_t addIdx;
1555  BooleanType addBoundNotConstraint;
1556  SubjectToStatus addStatus;
1557  BooleanType exchangeHappened = BT_FALSE;
1558 
1559 
1560  /* consistency checks */
1561  if ( constraints.getStatus( number ) == ST_INACTIVE )
1562  return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
1563 
1564  if ( ( number_idx < 0 ) || ( number_idx >= nAC ) )
1565  return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
1566 
1567  /* N) PERFORM ZERO CURVATURE TEST. */
1568  if (ensureNZC == BT_TRUE)
1569  {
1570  returnvalue = ensureNonzeroCurvature(BT_FALSE, number, exchangeHappened, addBoundNotConstraint, addIdx, addStatus);
1571 
1572  if (returnvalue != SUCCESSFUL_RETURN)
1573  return returnvalue;
1574  }
1575 
1576  /* save old constraint status and determinant of old S for flipping strategy */
1577  oldStatus = constraints.getStatus( number );
1578  oldDet = detS;
1579 
1580  /* I) UPDATE INDICES */
1581  tabularOutput.idxRemC = number;
1582  if ( constraints.moveActiveToInactive( number ) != SUCCESSFUL_RETURN )
1583  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
1584 
1585  /* Also update the Schur complement. */
1586 
1587  /* First check if this constraint had been added before. In that
1588  case delete this constraint from the Schur complement. */
1589  bool found = false;
1590  for ( int_t i=0; i<nS; i++ )
1591  {
1592  if ( schurUpdate[i] == SUT_ConAdded && number == schurUpdateIndex[i] )
1593  {
1594  if ( deleteFromSchurComplement( i, BT_TRUE ) != SUCCESSFUL_RETURN )
1595  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
1596  found = true;
1597  idxDeleted = i;
1598  sModType = 2;
1599  break;
1600  }
1601  }
1602  if ( !found )
1603  {
1604  if ( nS < 0 || nS==nSmax )
1605  {
1606  /* The schur complement has become too large, reset. */
1607  /* Don't check inertia here, may be corrected later! */
1608  returnValue retval = resetSchurComplement( BT_FALSE );
1609  if ( retval != SUCCESSFUL_RETURN )
1610  {
1611  if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
1612  MyPrintf( "In removeConstraint: KKT matrix singular when resetting Schur complement\n" );
1613  else if ( options.printLevel == PL_HIGH )
1614  MyPrintf( "In removeConstraint, resetSchurComplement failed with retval = %d\n", retval);
1615  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
1616  }
1617  found = true;
1618  sModType = 3;
1619  }
1620  else
1621  {
1622  /* If the constraint was not yet in Schur complement, add it now. */
1623  int_t nFRStart = boundsFreeStart.getLength();
1624  int_t nACStart = constraintsActiveStart.getLength();
1625  int_t* AC_idxStart;
1626  constraintsActiveStart.getNumberArray( &AC_idxStart );
1627 
1628  for ( int_t i=0; i<nACStart; i++ )
1629  if ( AC_idxStart[i] == number )
1630  {
1631  real_t one = 1.0;
1632  sparse_int_t pos = nFRStart+i;
1633  if ( addToSchurComplement( number, SUT_ConRemoved, 1, &pos, &one, 0, 0, 0, 0.0 ) != SUCCESSFUL_RETURN )
1634  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
1635  found = true;
1636  break;
1637  }
1638 
1639  sModType = 1;
1640  }
1641  }
1642 
1643  if ( !found )
1644  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
1645 
1646  /* Now we have a new Schur complement (might be smaller, larger, or empty). Update QR factorization. */
1647 
1648  /* Flipping bounds strategy */
1649  if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
1650  {
1651  if ( sModType == 1 )
1652  {/* Case 1: We added a row and column to S. */
1653 
1654  /* Check if a direction of negative curvature showed up, i.e. determinants have THE SAME sign */
1655  newDet = calcDetSchur( idxDeleted );
1656 
1657  if ( oldDet * newDet > 0 )
1658  {
1659  hessianType = HST_SEMIDEF;
1660 
1661  /* Restore old S */
1662  nS--;
1663 
1664  /* Flip bounds */
1665  tabularOutput.idxAddC = number;
1666  tabularOutput.excAddC = 2;
1667  switch ( oldStatus )
1668  {
1669  case ST_LOWER:
1670  constraints.moveInactiveToActive( number, ST_UPPER );
1671  ubA[number] = lbA[number];
1672  Ax_l[number] = -Ax_u[number];
1673  break;
1674  case ST_UPPER:
1675  constraints.moveInactiveToActive( number, ST_LOWER );
1676  lbA[number] = ubA[number];
1677  Ax_u[number] = -Ax_l[number];
1678  break;
1679  default:
1680  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
1681  }
1682  }
1683  else
1684  {/* Determinants have the correct sign, compute QR of new (larger) S */
1685  updateSchurQR( idxDeleted );
1686  }
1687  }
1688  else if ( sModType == 2 )
1689  {/* Case 2: We deleted a row and column of S. */
1690 
1691  /* Check if a direction of negative curvature showed up, i.e. determinants have DIFFERENT signs */
1692  newDet = calcDetSchur( idxDeleted );
1693 
1694  if ( oldDet * newDet < 0.0 )
1695  {
1696  hessianType = HST_SEMIDEF;
1697 
1698  /* Restore old S */
1699  undoDeleteFromSchurComplement( idxDeleted );
1700 
1701  /* Flip bounds */
1702  tabularOutput.idxAddC = number;
1703  tabularOutput.excAddC = 2;
1704  switch ( oldStatus )
1705  {
1706  case ST_LOWER:
1707  constraints.moveInactiveToActive( number, ST_UPPER );
1708  ubA[number] = lbA[number];
1709  Ax_l[number] = -Ax_u[number];
1710  break;
1711  case ST_UPPER:
1712  constraints.moveInactiveToActive( number, ST_LOWER );
1713  lbA[number] = ubA[number];
1714  Ax_u[number] = -Ax_l[number];
1715  break;
1716  default:
1717  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
1718  }
1719  }
1720  else
1721  {/* Determinants have the correct sign, compute QR of new (smaller) S */
1722  updateSchurQR( idxDeleted );
1723  }
1724  }
1725  else if ( sModType == 3 )
1726  {/* Case 3: S was reset. */
1727 
1728  /* Check inertia of new factorization given by the sparse solver: must be ( nFR, nAC, 0 ) */
1729  int_t neig = sparseSolver->getNegativeEigenvalues( );
1730  if( neig > getNAC( ) ) // Wrong inertia!
1731  {
1732  /* Flip bounds and update Schur complement */
1733  tabularOutput.idxAddC = number;
1734  tabularOutput.excAddC = 2;
1735  switch ( oldStatus )
1736  {
1737  case ST_LOWER:
1738  ubA[number] = lbA[number];
1739  Ax_l[number] = -Ax_u[number];
1740  addConstraint( number, ST_UPPER, BT_TRUE, BT_FALSE );
1741  break;
1742  case ST_UPPER:
1743  lbA[number] = ubA[number];
1744  Ax_u[number] = -Ax_l[number];
1745  addConstraint( number, ST_LOWER, BT_TRUE, BT_FALSE );
1746  break;
1747  default:
1748  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
1749  }
1750  }
1751 
1752  /* Check if flipping deleted the negative eigenvalue */
1753  if( correctInertia( ) )
1754  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
1755  }
1756  else
1757  {/* None of the three cases happened */
1758  return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
1759  }
1760  }
1761  else
1762  {/* No flipping strategy, update QR factorization of S */
1763  updateSchurQR( idxDeleted );
1764  }
1765 
1766  /* If reciprocal of condition number becomes to small, refactorize KKT matrix */
1767  if( rcondS < options.rcondSMin )
1768  {
1769  returnValue retval = resetSchurComplement( BT_TRUE );
1770  if ( retval != SUCCESSFUL_RETURN )
1771  {
1772  if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
1773  MyPrintf( "In removeConstraint: KKT matrix singular when resetting Schur complement\n" );
1774  else if ( options.printLevel == PL_HIGH )
1775  MyPrintf( "In removeConstraint, resetSchurComplement failed with retval = %d\n", retval);
1776  return THROWERROR( RET_ADDCONSTRAINT_FAILED );
1777  }
1778  }
1779 
1780  if ( exchangeHappened == BT_TRUE )
1781  {
1782  /* add bound or constraint */
1783 
1784  if ( addBoundNotConstraint )
1785  {
1786  addBound(addIdx, addStatus, BT_TRUE, BT_FALSE);
1787  tabularOutput.excAddB = 1;
1788  }
1789  else
1790  {
1791  addConstraint(addIdx, addStatus, BT_TRUE, BT_FALSE);
1792  tabularOutput.excAddC = 1;
1793  }
1794  }
1795 
1796  return SUCCESSFUL_RETURN;
1797 }
1798 
1799 
1800 /*
1801  * r e m o v e B o u n d
1802  */
1803 returnValue SQProblemSchur::removeBound( int_t number,
1804  BooleanType updateCholesky,
1805  BooleanType allowFlipping,
1806  BooleanType ensureNZC
1807  )
1808 {
1809  returnValue returnvalue = SUCCESSFUL_RETURN;
1810  int_t addIdx;
1811  BooleanType addBoundNotConstraint;
1812  SubjectToStatus addStatus;
1813  BooleanType exchangeHappened = BT_FALSE;
1814 
1815  int_t sModType = 0;
1816  int_t idxDeleted = -1;
1817  SubjectToStatus oldStatus;
1818  real_t oldDet, newDet;
1819 
1820  /* consistency checks */
1821  if ( bounds.getStatus( number ) == ST_INACTIVE )
1822  return THROWERROR( RET_BOUND_NOT_ACTIVE );
1823 
1824  if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
1825  ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
1826  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
1827  ( getStatus( ) == QPS_SOLVED ) )
1828  {
1829  return THROWERROR( RET_UNKNOWN_BUG );
1830  }
1831 
1832  /* N) PERFORM ZERO CURVATURE TEST. */
1833  if (ensureNZC == BT_TRUE)
1834  {
1835  returnvalue = ensureNonzeroCurvature(BT_TRUE, number, exchangeHappened, addBoundNotConstraint, addIdx, addStatus);
1836 
1837  if (returnvalue != SUCCESSFUL_RETURN)
1838  return returnvalue;
1839  }
1840 
1841  /* save old bound status and determinant of old S for flipping strategy */
1842  oldStatus = bounds.getStatus( number );
1843  oldDet = detS;
1844 
1845  /* I) UPDATE INDICES */
1846  tabularOutput.idxRemB = number;
1847  if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
1848  return THROWERROR( RET_REMOVEBOUND_FAILED );
1849 
1850  /* Also update the Schur complement. */
1851 
1852  /* First check if this variable had been fixed before. In that
1853  case delete this variable from the Schur complement. */
1854  bool found = false;
1855  for ( int_t i=0; i<nS; i++ )
1856  {
1857  if ( schurUpdate[i] == SUT_VarFixed && number == schurUpdateIndex[i] )
1858  {
1859  if ( deleteFromSchurComplement( i, BT_TRUE ) != SUCCESSFUL_RETURN )
1860  return THROWERROR( RET_REMOVEBOUND_FAILED );
1861  found = true;
1862  idxDeleted = i;
1863  sModType = 2;
1864  break;
1865  }
1866  }
1867  if ( !found )
1868  {
1869  if ( nS < 0 || nS==nSmax )
1870  {
1871  /* The schur complement has become too large, reset. */
1872  /* Don't correct inertia here, may be corrected by flipping bounds! */
1873  returnValue retval = resetSchurComplement( BT_FALSE );
1874  if ( retval != SUCCESSFUL_RETURN )
1875  {
1876  if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
1877  MyPrintf( "In removeBound: KKT matrix singular when resetting Schur complement\n" );
1878  else if ( options.printLevel == PL_HIGH )
1879  MyPrintf( "In removeBound, resetSchurComplement failed with retval = %d\n", retval);
1880  return THROWERROR( RET_REMOVEBOUND_FAILED );
1881  }
1882  found = true;
1883  sModType = 3;
1884  }
1885  else
1886  {
1887  /* If the variable was not yet in Schur complement, add it now. */
1888  int_t nFRStart = boundsFreeStart.getLength();
1889  int_t nACStart = constraintsActiveStart.getLength();
1890  int_t* FR_idxStart;
1891  boundsFreeStart.getNumberArray( &FR_idxStart );
1892  int_t* AC_idxStart;
1893  constraintsActiveStart.getNumberArray( &AC_idxStart );
1894 
1895  int_t numNonzerosM = 0;
1896  sparse_int_t* Mpos = new sparse_int_t[nFRStart+nACStart+nS]; // This is an overestimate
1897  real_t* Mvals = new real_t[nFRStart+nACStart+nS];
1898  int_t numNonzerosN = 0;
1899  sparse_int_t* Npos = new sparse_int_t[nFRStart+nACStart+nS]; // This is an overestimate
1900  real_t* Nvals = new real_t[nFRStart+nACStart+nS];
1901  real_t N_diag;
1902 
1903  int_t* irn = new int_t[nFRStart+nACStart+nS+1];
1904  int_t* jcn = new int_t[nFRStart+nACStart+nS+1];
1905  real_t* vals = new real_t[nFRStart+nACStart+nS+1];
1906  int_t iLength;
1907  int_t* iNumber = new int_t[nFRStart+nACStart+nS+1];
1908  int_t numNonzeros;
1909  int_t* iSIdx = new int_t[nS];
1910 
1911  /* First the Hessian part. */
1912  real_t regularisation = options.epsRegularisation;
1913  switch ( hessianType )
1914  {
1915  case HST_ZERO:
1916  N_diag = regularisation;
1917  break;
1918 
1919  case HST_IDENTITY:
1920  N_diag = 1.0 + regularisation;
1921  break;
1922 
1923  default:
1924  N_diag = regularisation;
1925  for ( int_t i=0; i<nFRStart; i++ )
1926  iNumber[i] = FR_idxStart[i];
1927  iLength = nFRStart;
1928  for ( int_t i=0; i<nS; i++ )
1929  if ( schurUpdate[i] == SUT_VarFreed )
1930  {
1931  iNumber[iLength] = schurUpdateIndex[i];
1932  iSIdx[iLength-nFRStart] = i;
1933  iLength++;
1934  }
1935  iNumber[iLength++] = number;
1936 
1937  H->getSparseSubmatrix( iLength, iNumber, 1, &number, 0, 0, numNonzeros, irn, jcn, vals );
1938 
1939  for ( int_t i=0; i<numNonzeros; i++ )
1940  {
1941  if ( irn[i] < nFRStart )
1942  {
1943  Mpos[numNonzerosM] = irn[i];
1944  Mvals[numNonzerosM] = vals[i];
1945  numNonzerosM++;
1946  }
1947  else if ( irn[i] != iLength-1 )
1948  {
1949  Npos[numNonzerosN] = iSIdx[irn[i] - nFRStart];
1950  Nvals[numNonzerosN] = vals[i];
1951  numNonzerosN++;
1952  }
1953  else
1954  N_diag += vals[i];
1955  }
1956  break;
1957  }
1958 
1959  if ( constraintProduct != 0 )
1960  {
1961  MyPrintf( "In SQProblemSchur::removeBound, constraintProduct not yet implemented.\n");
1962  return THROWERROR(RET_NOT_YET_IMPLEMENTED);
1963  }
1964 
1965  for ( int_t i=0; i<nACStart; i++ )
1966  iNumber[i] = AC_idxStart[i];
1967  iLength = nACStart;
1968  for ( int_t i=0; i<nS; i++ )
1969  if ( schurUpdate[i] == SUT_ConAdded )
1970  {
1971  iNumber[iLength] = schurUpdateIndex[i];
1972  iSIdx[iLength-nACStart] = i;
1973  iLength++;
1974  }
1975 
1976  A->getSparseSubmatrix( iLength, iNumber, 1, &number, 0, 0, numNonzeros, irn, jcn, vals );
1977 
1978  for ( int_t i=0; i<numNonzeros; i++ )
1979  {
1980  if ( irn[i] < nACStart )
1981  {
1982  Mpos[numNonzerosM] = irn[i] + nFRStart;
1983  Mvals[numNonzerosM] = vals[i];
1984  numNonzerosM++;
1985  }
1986  else
1987  {
1988  Npos[numNonzerosN] = iSIdx[irn[i] - nACStart];
1989  Nvals[numNonzerosN] = vals[i];
1990  numNonzerosN++;
1991  }
1992  }
1993 
1994  delete [] iSIdx;
1995  delete [] iNumber;
1996  delete [] vals;
1997  delete [] jcn;
1998  delete [] irn;
1999 
2000  returnvalue = addToSchurComplement( number, SUT_VarFreed, numNonzerosM, Mpos, Mvals, numNonzerosN, Npos, Nvals, N_diag );
2001 
2002  delete [] Mvals;
2003  delete [] Mpos;
2004  delete [] Nvals;
2005  delete [] Npos;
2006 
2007  if ( returnvalue != SUCCESSFUL_RETURN )
2008  return THROWERROR( RET_REMOVEBOUND_FAILED );
2009 
2010  found = true;
2011  sModType = 1;
2012  }
2013  }
2014 
2015  if ( !found )
2016  return THROWERROR( RET_REMOVEBOUND_FAILED );
2017 
2018  /* Now we have a new Schur complement (might be smaller, larger, or empty). Update QR factorization. */
2019 
2020  /* Flipping bounds strategy */
2021  if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
2022  {
2023  if ( sModType == 1 )
2024  {/* Case 1: We added a row and column to S. */
2025 
2026  /* Check if a direction of negative curvature showed up, i.e. determinants have THE SAME sign */
2027  newDet = calcDetSchur( idxDeleted );
2028 
2029  if ( oldDet * newDet > 0.0 )
2030  {
2031  hessianType = HST_SEMIDEF;
2032 
2033  /* Restore old S */
2034  nS--;
2035 
2036  /* Flip bounds */
2037  tabularOutput.idxAddB = number;
2038  tabularOutput.excAddB = 2;
2039  switch ( oldStatus )
2040  {
2041  case ST_LOWER:
2042  bounds.moveFreeToFixed( number, ST_UPPER );
2043  ub[number] = lb[number];
2044  break;
2045  case ST_UPPER:
2046  bounds.moveFreeToFixed( number, ST_LOWER );
2047  lb[number] = ub[number];
2048  break;
2049  default:
2050  return THROWERROR( RET_MOVING_BOUND_FAILED );
2051  }
2052  }
2053  else
2054  {/* Determinants have the correct sign, compute QR of new (larger) S */
2055  updateSchurQR( idxDeleted );
2056  }
2057  }
2058  else if ( sModType == 2 )
2059  {/* Case 2: We deleted a row and column of S. */
2060 
2061  /* Check if a direction of negative curvature showed up, i.e. determinants have DIFFERENT signs */
2062  newDet = calcDetSchur( idxDeleted );
2063 
2064  if ( oldDet * newDet < 0.0 )
2065  {
2066  hessianType = HST_SEMIDEF;
2067 
2068  /* Restore old S */
2069  undoDeleteFromSchurComplement( idxDeleted );
2070 
2071  /* Flip bounds */
2072  tabularOutput.idxAddB = number;
2073  tabularOutput.excAddB = 2;
2074  switch ( oldStatus )
2075  {
2076  case ST_LOWER:
2077  bounds.moveFreeToFixed( number, ST_UPPER );
2078  ub[number] = lb[number];
2079  break;
2080  case ST_UPPER:
2081  bounds.moveFreeToFixed( number, ST_LOWER );
2082  lb[number] = ub[number];
2083  break;
2084  default:
2085  return THROWERROR( RET_MOVING_BOUND_FAILED );
2086  }
2087  }
2088  else
2089  {/* Determinants have the correct sign, compute QR of new (smaller) S */
2090  updateSchurQR( idxDeleted );
2091  }
2092  }
2093  else if ( sModType == 3 )
2094  {/* Case 3: S was reset. */
2095 
2096  /* Check inertia of new factorization given by the sparse solver: must be ( nFR, nAC, 0 ) */
2097  int_t neig = sparseSolver->getNegativeEigenvalues( );
2098  if( neig > getNAC( ) ) // Wrong inertia, flip bounds!
2099  {
2100  /* Flip bounds and update Schur complement */
2101  tabularOutput.idxAddB = number;
2102  tabularOutput.excAddB = 2;
2103  switch ( oldStatus )
2104  {
2105  case ST_LOWER:
2106  ub[number] = lb[number];
2107  addBound( number, ST_UPPER, BT_TRUE, BT_FALSE );
2108  break;
2109  case ST_UPPER:
2110  lb[number] = ub[number];
2111  addBound( number, ST_LOWER, BT_TRUE, BT_FALSE );
2112  break;
2113  default:
2114  return THROWERROR( RET_MOVING_BOUND_FAILED );
2115  }
2116  }
2117 
2118  /* Check if flipping deleted the negative eigenvalue */
2119  if( correctInertia( ) )
2120  return THROWERROR( RET_REMOVEBOUND_FAILED );
2121  }
2122  else
2123  {/* None of the three cases happened */
2124  return THROWERROR( RET_REMOVEBOUND_FAILED );
2125  }
2126  }
2127  else
2128  {/* No flipping strategy, update QR factorization of S */
2129  updateSchurQR( idxDeleted );
2130  }
2131 
2132  /* If reciprocal of condition number becomes to small, refactorize KKT matrix */
2133  if( rcondS < options.rcondSMin )
2134  {
2135  returnValue retval = resetSchurComplement( BT_TRUE );
2136  if ( retval != SUCCESSFUL_RETURN )
2137  {
2138  if ( retval == RET_KKT_MATRIX_SINGULAR && options.printLevel == PL_HIGH )
2139  MyPrintf( "In removeBound: KKT matrix singular when resetting Schur complement\n" );
2140  else if ( options.printLevel == PL_HIGH )
2141  MyPrintf( "In removeBound, resetSchurComplement failed with retval = %d\n", retval);
2142  return THROWERROR( RET_ADDCONSTRAINT_FAILED );
2143  }
2144  }
2145 
2146  if ( exchangeHappened == BT_TRUE )
2147  {
2148  /* add bound or constraint */
2149 
2150  if ( addBoundNotConstraint )
2151  {
2152  addBound(addIdx, addStatus, BT_TRUE, BT_FALSE);
2153  tabularOutput.excAddB = 1;
2154  }
2155  else
2156  {
2157  addConstraint(addIdx, addStatus, BT_TRUE, BT_FALSE);
2158  tabularOutput.excAddC = 1;
2159  }
2160  }
2161 
2162  return SUCCESSFUL_RETURN;
2163 }
2164 
2165 
2166 /*
2167  * s e t u p T Q f a c t o r i s a t i o n
2168  */
2169 returnValue SQProblemSchur::backsolveT( const real_t* const b, BooleanType transposed, real_t* const a ) const
2170 {
2171  return THROWERROR( RET_UNKNOWN_BUG );
2172 }
2173 
2174 
2175 /*
2176  * b a c k s o l v e R
2177  */
2178 returnValue SQProblemSchur::backsolveR( const real_t* const b, BooleanType transposed, real_t* const a ) const
2179 {
2180  return THROWERROR( RET_UNKNOWN_BUG );
2181 }
2182 
2183 
2184 /*
2185  * b a c k s o l v e R
2186  */
2187 returnValue SQProblemSchur::backsolveR( const real_t* const b, BooleanType transposed, BooleanType removingBound, real_t* const a ) const
2188 {
2189  return THROWERROR( RET_UNKNOWN_BUG );
2190 }
2191 
2192 
2193 /*
2194  * c a l c D e t S c h u r
2195  */
2196 real_t SQProblemSchur::calcDetSchur( int_t idxDel )
2197 {
2198  if ( nS <= 0 )
2199  return 1.0;
2200 
2201  real_t newDet;
2202  int_t i, j;
2203  real_t c, s, nu;
2204 
2205  /* Case 1: S has been bordered by one row and column */
2206  if( idxDel < 0 )
2207  {
2208  /* Do a solve with the old S to check determinant of new (bordered) S */
2209  real_t *temp1 = new real_t[nS-1];
2210  real_t *temp2 = new real_t[nS-1];
2211  for( i=0; i<nS-1; i++ )
2212  temp1[i] = S[i + (nS-1)*nSmax];
2213  backsolveSchurQR( nS-1, temp1, 1, temp2 );
2214 
2215  newDet = S[(nS-1) + (nS-1)*nSmax];
2216  for( i=0; i<nS-1; i++ )
2217  newDet -= temp1[i]*temp2[i];
2218  newDet *= detS;
2219  delete [] temp1;
2220  delete [] temp2;
2221  }
2222  /* Case 2: row and column idxDel have been deleted from S */
2223  else
2224  {
2225  const int_t dim = nS+1;
2226  real_t *tempR = new real_t[dim*(dim-1)];
2227  real_t *tempColQ = new real_t[dim];
2228 
2229  /* Copy current R without column idxDel*/
2230  for( j=0; j<idxDel; j++ )
2231  for( i=0; i<dim; i++ )
2232  tempR[i+j*dim] = R_[i+j*nSmax];
2233  for( j=idxDel; j<dim-1; j++ )
2234  for( i=0; i<dim; i++ )
2235  tempR[i+j*dim] = R_[i+(j+1)*nSmax];
2236  /* Copy row idxDel of Q */
2237  for( j=0; j<dim; j++ )
2238  tempColQ[j] = Q_[idxDel+j*nSmax];
2239 
2240  /* Bring tempR to triangular form with nS-idxDel Givens rotations */
2241  for ( i=idxDel; i<nS; i++ )
2242  {
2243  computeGivens( tempR[i+i*dim], tempR[(i+1)+i*dim], tempR[i+i*dim], tempR[(i+1)+i*dim], c, s );
2244  nu = s/(1.0+c);
2246  for ( j=i+1; j<nS; j++ )
2247  applyGivens( c, s, nu, tempR[i+j*dim], tempR[(i+1)+j*dim], tempR[i+j*dim], tempR[(i+1)+j*dim] );
2248 
2249  /* Simultaneously transform relevant column of Q**T */
2250  applyGivens( c, s, nu, tempColQ[i], tempColQ[i+1], tempColQ[i], tempColQ[i+1] );
2251  }
2252 
2253  /* Delete row: nS Givens rotations to transform last column (and row!) of (old) Q**T to getAbs((nS+1)-th unity vector) */
2254  for ( i=nS; i>0; i-- )
2255  {
2256  computeGivens( tempColQ[nS], tempColQ[i-1], tempColQ[nS], tempColQ[i-1], c, s );
2257  nu = s/(1.0+c);
2258 
2259  /* Simultaneously transform diagonal elements of R (coldim is already one less than Q) */
2260  applyGivens( c, s, nu, tempR[nS+(i-1)*dim], tempR[(i-1)+(i-1)*dim], tempR[nS+(i-1)*dim], tempR[(i-1)+(i-1)*dim] );
2261  }
2262 
2263  /* Note that we implicitly did a row permutation of Q.
2264  * If we did an odd permutation AND deleted a positive unity vector or
2265  * if we did an even permutation AND deleted a negative unity vector, then det(Q)=-1
2266  * ->Change signs of first column of Q and first row of R */
2267  if ( (( (nS - idxDel) % 2 == 1 ) && ( tempColQ[nS] > 0.0 )) ||
2268  (( (nS - idxDel) % 2 == 0 ) && ( tempColQ[nS] < 0.0 )) )
2269  {
2270  tempR[0] = -tempR[0];
2271  }
2272 
2273  newDet = 1.0;
2274  //for( i=0; i<nS; i++ )
2275  //newDet *= tempR[i+i*dim];
2276  for( i=0; i<nS; i++ )
2277  if( tempR[i+i*dim] < 0.0 ) newDet = -newDet;
2278  delete [] tempR;
2279  delete [] tempColQ;
2280  }
2281 
2282  return newDet;
2283 }
2284 
2285 
2286 /*
2287  * u p d a t e S c h u r Q R
2288  */
2289 returnValue SQProblemSchur::updateSchurQR( int_t idxDel )
2290 {
2291  int_t i, j;
2292  real_t c, s, nu;
2293 
2294  if ( nS <= 0 )
2295  {
2296  detS = 1.0;
2297  rcondS = 1.0;
2298  return SUCCESSFUL_RETURN;
2299  }
2300 
2301  /* Case 1: S has been bordered by one row and column */
2302  if ( idxDel < 0 )
2303  {
2304  /* I: Augment Q**T by nS-th unity vector (row and column) */
2305  for ( i=0; i<nS; i++ )
2306  {
2307  Q_[i+(nS-1)*nSmax] = 0.0;
2308  Q_[(nS-1)+i*nSmax] = 0.0;
2309  }
2310  Q_[(nS-1)+(nS-1)*nSmax] = 1.0;
2311 
2312  /* IIa: Augment rows of R by last row of S */
2313  for ( i=0; i<nS; i++ )
2314  R_[(nS-1)+i*nSmax] = S[(nS-1)+i*nSmax];
2315 
2316  /* IIb: Augment columns of R by Q**T * S[nS,:] */
2317  for ( i=0; i<nS; i++ )
2318  {
2319  R_[i+(nS-1)*nSmax] = 0.0;
2320  for ( j=0; j<nS; j++ )
2321  R_[i+(nS-1)*nSmax] += Q_[j+i*nSmax] * S[j+(nS-1)*nSmax];
2322  }
2323 
2324  /* III: Restore triangular form of R by nS-1 Givens rotations */
2325  for ( i=0; i<nS-1; i++ )
2326  {
2327  computeGivens( R_[i+i*nSmax], R_[(nS-1)+i*nSmax], R_[i+i*nSmax], R_[(nS-1)+i*nSmax], c, s );
2328  nu = s/(1.0+c);
2329  for ( j=i+1; j<nS; j++ )
2330  applyGivens( c, s, nu, R_[i+j*nSmax], R_[(nS-1)+j*nSmax], R_[i+j*nSmax], R_[(nS-1)+j*nSmax] );
2331 
2332  /* Simultaneously transform Q**T */
2333  for ( j=0; j<nS; j++ )
2334  applyGivens( c, s, nu, Q_[j+i*nSmax], Q_[j+(nS-1)*nSmax], Q_[j+i*nSmax], Q_[j+(nS-1)*nSmax] );
2335  }
2336  }
2337  /* Case 2: row and column idxDel have been deleted from S */
2338  else
2339  {
2340  /* I: Delete column idxDel of R */
2341  for ( j=idxDel; j<nS; j++ )
2342  for ( i=0; i<nS+1; i++ )
2343  R_[i+j*nSmax] = R_[i+(j+1)*nSmax];
2344 
2345  /* II: Bring R back to triangular form with nS-idxDel Givens rotations */
2346  for ( i=idxDel; i<nS; i++ )
2347  {
2348  computeGivens( R_[i+i*nSmax], R_[(i+1)+i*nSmax], R_[i+i*nSmax], R_[(i+1)+i*nSmax], c, s );
2349  nu = s/(1.0+c);
2350  for ( j=i+1; j<nS; j++ )
2351  applyGivens( c, s, nu, R_[i+j*nSmax], R_[(i+1)+j*nSmax], R_[i+j*nSmax], R_[(i+1)+j*nSmax] );
2352 
2353  /* Simultaneously transform (old) Q**T (coldim is one larger)*/
2354  for ( j=0; j<nS+1; j++ )
2355  applyGivens( c, s, nu, Q_[j+i*nSmax], Q_[j+(i+1)*nSmax], Q_[j+i*nSmax], Q_[j+(i+1)*nSmax] );
2356  }
2357 
2358  /* III: Permute rows of Q: move row idxDel to position nS */
2359  real_t temp;
2360  for ( j=0; j<nS+1; j++ )
2361  {
2362  temp = Q_[idxDel+j*nSmax];
2363  for ( i=idxDel; i<nS; i++ )
2364  Q_[i+j*nSmax] = Q_[(i+1)+j*nSmax];
2365  Q_[nS+j*nSmax] = temp;
2366  }
2367 
2368  /* IV: Delete row: nS Givens rotations to transform last column (and row!) of (old) Q**T to getAbs((nS+1)-th unity vector) */
2369  for ( i=nS; i>0; i-- )
2370  {
2371  computeGivens( Q_[nS+nS*nSmax], Q_[nS+(i-1)*nSmax], Q_[nS+nS*nSmax], Q_[nS+(i-1)*nSmax], c, s );
2372  nu = s/(1.0+c);
2373  for ( j=0; j<nS; j++ )
2374  applyGivens( c, s, nu, Q_[j+nS*nSmax], Q_[j+(i-1)*nSmax], Q_[j+nS*nSmax], Q_[j+(i-1)*nSmax] );
2375 
2376  /* Simultaneously transform R (coldim is already one less than Q) */
2377  for ( j=i-1; j<nS; j++ )
2378  applyGivens( c, s, nu, R_[nS+j*nSmax], R_[(i-1)+j*nSmax], R_[nS+j*nSmax], R_[(i-1)+j*nSmax] );
2379  }
2380 
2381  /* If we did an odd permutation AND deleted a positive unity vector or
2382  * if we did an even permutation AND deleted a negative unity vector, then det(Q)=-1
2383  * ->Change signs of first column of Q and first row of R s.t. we always maintain det(Q)=1 */
2384  if ( (( (nS - idxDel) % 2 == 1 ) && ( Q_[nS+nS*nSmax] > 0.0 )) ||
2385  (( (nS - idxDel) % 2 == 0 ) && ( Q_[nS+nS*nSmax] < 0.0 )) )
2386  {
2387  for ( i=0; i<nS+1; i++ )
2388  Q_[i] = -Q_[i];
2389  for ( i=0; i<nS; i++ )
2390  R_[i*nSmax] = -R_[i*nSmax];
2391  }
2392  }
2393 
2394  /* Compute determinant */
2395  detS = 1.0;
2396  //for ( i=0; i<nS; i++ )
2397  //detS *= R_[i+i*nSmax];
2398  for ( i=0; i<nS; i++ )
2399  if( R_[i+i*nSmax] < 0.0 ) detS = -detS;
2400 
2401  /* Estimate condition number of R (= condition number of S)*/
2402  real_t *WORK;
2403  unsigned long N = (unsigned long)nS;
2404  unsigned long LDA = (unsigned long)nSmax;
2405  unsigned long *IWORK;
2406  long INFO = 0;
2407  IWORK = new unsigned long[N];
2408  WORK = new real_t[3*N];
2409  TRCON( "1", "U", "N", &N, R_, &LDA, &rcondS, WORK, IWORK, &INFO );
2410  if ( INFO != 0 )
2411  {
2412  MyPrintf( "TRCON returns INFO = %d\n",(int)INFO );
2413  }
2414 
2415  if ( options.printLevel == PL_HIGH )
2416  MyPrintf( "1/cond(S) = %23.16e.\n", rcondS );
2417 
2418  delete[] IWORK;
2419  delete[] WORK;
2420 
2421  return SUCCESSFUL_RETURN;
2422 }
2423 
2424 
2425 /*
2426  * b a c k s o l v e S c h u r Q R
2427  */
2428 returnValue SQProblemSchur::backsolveSchurQR( int_t dimS, const real_t* const rhs, int_t dimRhs, real_t* const sol )
2429 {
2430  if( dimS < 1 || dimRhs < 1 )
2431  return SUCCESSFUL_RETURN;
2432 
2433  if( dimRhs > 1 )
2434  {
2435  MyPrintf("backsolve not implemented for dimRhs = %d\n", dimRhs);
2436  return RET_QR_FACTORISATION_FAILED;
2437  }
2438 
2439  int_t i, j;
2440  long INFO = 0;
2441  unsigned long NRHS = 1;
2442  unsigned long M = (unsigned long)dimS;
2443  unsigned long LDA = (unsigned long)nSmax;
2444  unsigned long LDC = (unsigned long)dimS;
2445 
2446  for( i=0; i<dimS; i++ )
2447  sol[i] = 0.0;
2448 
2449  /* Compute sol = Q**T * rhs */
2450  for( i=0; i<dimS; i++ )
2451  for( j=0; j<dimS; j++ )
2452  sol[i] += Q_[j+i*nSmax] * rhs[j];
2453 
2454  /* Solve Rx = sol */
2455  TRTRS( "U", "N", "N", &M, &NRHS, R_, &LDA, sol, &LDC, &INFO );
2456  if ( INFO != 0 )
2457  {
2458  MyPrintf("TRTRS returns INFO = %d\n", INFO);
2459  return RET_QR_FACTORISATION_FAILED;
2460  }
2461 
2462  return SUCCESSFUL_RETURN;
2463 }
2464 
2465 
2466 returnValue SQProblemSchur::stepCalcRhs( int_t nFR, int_t nFX, int_t nAC, int_t* FR_idx, int_t* FX_idx, int_t* AC_idx, real_t& rhs_max,
2467  const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
2468  const real_t* const delta_lb, const real_t* const delta_ub,
2469  BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
2470  real_t* const delta_xFX, real_t* const delta_xFR,
2471  real_t* const delta_yAC, real_t* const delta_yFX
2472  )
2473 {
2474  int_t i, ii;
2475  returnValue retval;
2476 
2477  if ( nS < 0 )
2478  {
2479  retval = resetSchurComplement( BT_FALSE );
2480  if (retval != SUCCESSFUL_RETURN)
2481  {
2482  MyPrintf( "In SQProblemSchur::stepCalcRhs, resetSchurComplement returns %d\n", retval);
2483  return THROWERROR( retval );
2484  }
2485  }
2486 
2487  /* tempA and tempB hold the residuals in gFR and bA (= lbA or ubA)
2488  * delta_xFR, delta_yAC hold the steps that get refined */
2489  for ( i=0; i<nFR; ++i )
2490  {
2491  ii = FR_idx[i];
2492  tempA[i] = delta_g[ii];
2493  delta_xFR[i] = 0.0;
2494  }
2495  for ( i=0; i<nAC; ++i )
2496  delta_yAC[i] = 0.0;
2497  if ( Delta_bC_isZero == BT_FALSE )
2498  {
2499  for ( i=0; i<nAC; ++i )
2500  {
2501  ii = AC_idx[i];
2502  if ( constraints.getStatus( ii ) == ST_LOWER )
2503  tempB[i] = delta_lbA[ii];
2504  else
2505  tempB[i] = delta_ubA[ii];
2506  }
2507  }
2508  else
2509  {
2510  for ( i=0; i<nAC; ++i )
2511  tempB[i] = 0.0;
2512  }
2513  if ( ( hessianType != HST_IDENTITY ) && ( hessianType != HST_ZERO ) )
2514  {
2515  /* tempA becomes RHS for reduced augmented system, gFR+H_FX*delta_xFR */
2516  H->times(bounds.getFree(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, tempA, nFR);
2517  }
2518  /* tempB becomes RHS for reduced augmented system, bA-A_CX*delta_xFR */
2519  A->times(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, tempB, nAC);
2520 
2521  /* If iterative refinement is requested, compute max-norm of RHS for termination test. */
2522  rhs_max = 0.0;
2523  if ( options.numRefinementSteps > 0 )
2524  {
2525  for ( i=0; i<nFR; i++ )
2526  rhs_max = getMax(rhs_max, getAbs(tempA[i]));
2527  for ( i=0; i<nAC; i++ )
2528  rhs_max = getMax(rhs_max, getAbs(tempB[i]));
2529  }
2530  return SUCCESSFUL_RETURN;
2531 }
2532 
2533 returnValue SQProblemSchur::stepCalcReorder(int_t nFR, int_t nAC, int_t* FR_idx, int_t* AC_idx, int_t nFRStart, int_t nACStart, int_t* FR_idxStart, int_t* AC_idxStart, int_t* FR_iSort, int_t* FR_iSortStart, int_t* AC_iSort, int_t* AC_iSortStart, real_t* rhs)
2534 {
2535  int_t i, ii;
2536  /* Reorder information for the new to the old free variables. */
2537  i = 0;
2538  ii = 0;
2539  while ( ii < nFRStart )
2540  {
2541  if ( i == nFR )
2542  rhs[FR_iSortStart[ii++]] = 0.0;
2543  else
2544  {
2545  int_t idx = FR_idx[FR_iSort[i]];
2546  int_t idxStart = FR_idxStart[FR_iSortStart[ii]];
2547 
2548  if ( idx == idxStart )
2549  rhs[FR_iSortStart[ii++]] = -tempA[FR_iSort[i++]];
2550  else if ( idx < idxStart )
2551  i++;
2552  else
2553  rhs[FR_iSortStart[ii++]] = 0.0;
2554  }
2555  }
2556  /* Reorder information for the new to the old active constraints. */
2557  i = 0;
2558  ii = 0;
2559  while ( ii < nACStart )
2560  {
2561  if ( i == nAC )
2562  rhs[nFRStart+AC_iSortStart[ii++]] = 0.0;
2563  else
2564  {
2565  int_t idx = AC_idx[AC_iSort[i]];
2566  int_t idxStart = AC_idxStart[AC_iSortStart[ii]];
2567 
2568  if ( idx == idxStart )
2569  rhs[nFRStart+AC_iSortStart[ii++]] = tempB[AC_iSort[i++]];
2570  else if ( idx < idxStart )
2571  i++;
2572  else
2573  rhs[nFRStart+AC_iSortStart[ii++]] = 0.0;
2574  }
2575  }
2576  return SUCCESSFUL_RETURN;
2577 }
2578 
2579 returnValue SQProblemSchur::stepCalcBacksolveSchur( int_t nFR, int_t nFX, int_t nAC, int_t* FR_idx, int_t* FX_idx, int_t* AC_idx, int_t dim, real_t* rhs, real_t* sol )
2580 {
2581  returnValue retval;
2582  int_t i, ii;
2583 
2584  real_t* q = new real_t[nS];
2585 
2586  /* Compute extra compoments of the RHS */
2587  for ( ii=0; ii<nS; ii++ )
2588  {
2589  int_t idx = schurUpdateIndex[ii];
2590  switch ( schurUpdate[ii] ) // TODO: All the loops below could be done faster by binary search or so
2591  {
2592  case SUT_VarFixed:
2593  q[ii] = 0.0;
2594  break;
2595 
2596  case SUT_VarFreed:
2597  /* Find index of freed variable */
2598  for( i=0; i<nFR; ++i )
2599  if ( FR_idx[i] == idx )
2600  {
2601  q[ii] = -tempA[i];
2602  break;
2603  }
2604  break;
2605 
2606  case SUT_ConAdded:
2607  /* Find index of added constraint */
2608  for( i=0; i<nAC; ++i )
2609  if ( AC_idx[i] == idx )
2610  {
2611  q[ii] = tempB[i];
2612  break;
2613  }
2614  break;
2615 
2616  case SUT_ConRemoved:
2617  q[ii] = 0.0;
2618  break;
2619 
2620  default:
2621  return THROWERROR( RET_UNKNOWN_BUG );
2622  }
2623  }
2624 
2625  /* compute q = M^T K^{-1} r - q */
2626  computeMTransTimes(1.0, sol, -1.0, q);
2627 
2628  /* Solve linear system with Schur complement. */
2629  real_t* p = new real_t[nS];
2630  backsolveSchurQR( nS, q, 1, p );
2631 
2632  computeMTimes(-1.0, p, 1.0, rhs);
2633 
2634  retval = sparseSolver->solve(dim, rhs, sol);
2635  if (retval != SUCCESSFUL_RETURN)
2636  {
2637  MyPrintf( "sparseSolver->solve (second time) failed.\n");
2638  return THROWERROR(RET_MATRIX_FACTORISATION_FAILED); // TODO: Different return code
2639  }
2640 
2641  /* Transfer extra compoments of the Schur complement solution to the correct place. */
2642  for ( ii=0; ii<nS; ii++ )
2643  {
2644  int_t idx = schurUpdateIndex[ii];
2645  switch ( schurUpdate[ii] ) // TODO: All the loops below could be done faster by binary search or so
2646  {
2647  case SUT_VarFixed:
2648  break;
2649 
2650  case SUT_VarFreed:
2651  /* Find index of freed variable */
2652  for( i=0; i<nFR; ++i )
2653  if ( FR_idx[i] == idx )
2654  {
2655  delta_xFR_TMP[i] = p[ii];
2656  break;
2657  }
2658  break;
2659 
2660  case SUT_ConAdded:
2661  /* Find index of added constraint */
2662  for( i=0; i<nAC; ++i )
2663  if ( AC_idx[i] == idx )
2664  {
2665  delta_yAC_TMP[i] = -p[ii];
2666  break;
2667  }
2668  break;
2669 
2670  case SUT_ConRemoved:
2671  break;
2672 
2673  default:
2674  return THROWERROR( RET_UNKNOWN_BUG );
2675  }
2676  }
2677 
2678  delete [] p;
2679  delete [] q;
2680  return SUCCESSFUL_RETURN;
2681 }
2682 
2683 returnValue SQProblemSchur::stepCalcReorder2(int_t nFR, int_t nAC, int_t* FR_idx, int_t* AC_idx, int_t nFRStart, int_t nACStart, int_t* FR_idxStart, int_t* AC_idxStart, int_t* FR_iSort, int_t* FR_iSortStart, int_t* AC_iSort, int_t* AC_iSortStart, real_t* sol, real_t* const delta_xFR, real_t* const delta_yAC)
2684 {
2685  int_t i, ii;
2686  i = 0;
2687  ii = 0;
2688  while ( ii < nFRStart && i < nFR )
2689  {
2690  int_t idx = FR_idx[FR_iSort[i]];
2691  int_t idxStart = FR_idxStart[FR_iSortStart[ii]];
2692 
2693  if ( idx == idxStart )
2694  delta_xFR_TMP[FR_iSort[i++]] = sol[FR_iSortStart[ii++]];
2695  else if ( idx < idxStart )
2696  i++;
2697  else
2698  ii++;
2699  }
2700  /* Transfer Schur complement solution for the active constraint multipliers to the correct places */
2701  i = 0;
2702  ii = 0;
2703  while ( ii < nACStart && i < nAC )
2704  {
2705  int_t idx = AC_idx[AC_iSort[i]];
2706  int_t idxStart = AC_idxStart[AC_iSortStart[ii]];
2707 
2708  if ( idx == idxStart )
2709  delta_yAC_TMP[AC_iSort[i++]] = -sol[nFRStart+AC_iSortStart[ii++]];
2710  else if ( idx < idxStart )
2711  i++;
2712  else
2713  ii++;
2714  }
2715 
2716  /* refine the solution found so far */
2717  for ( i=0; i<nFR; ++i )
2718  delta_xFR[i] += delta_xFR_TMP[i];
2719  for ( i=0; i<nAC; ++i )
2720  delta_yAC[i] += delta_yAC_TMP[i];
2721  return SUCCESSFUL_RETURN;
2722 }
2723 
2724 returnValue SQProblemSchur::stepCalcResid(int_t nFR, int_t nFX, int_t nAC, int_t* FR_idx, int_t* FX_idx, int_t* AC_idx, BooleanType Delta_bC_isZero, real_t* const delta_xFX, real_t* const delta_xFR, real_t* const delta_yAC, const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA, real_t& rnrm)
2725 {
2726  int_t i, ii;
2727  /* compute residuals in tempA and tempB, and max-norm */
2728  for ( i=0; i<nFR; ++i )
2729  {
2730  ii = FR_idx[i];
2731  tempA[i] = delta_g[ii];
2732  }
2733 
2734  switch ( hessianType )
2735  {
2736  case HST_ZERO:
2737  break;
2738 
2739  case HST_IDENTITY:
2740  for ( i=0; i<nFR; ++i )
2741  tempA[i] += delta_xFR[i];
2742  break;
2743 
2744  default:
2745  H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, tempA, nFR);
2746  H->times(bounds.getFree(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, tempA, nFR);
2747  break;
2748  }
2749 
2750  for ( i=0; i<nFR; ++i )
2751  tempA[i] += options.epsRegularisation*delta_xFR[i];
2752 
2753  A->transTimes(constraints.getActive(), bounds.getFree(), 1, -1.0, delta_yAC, nAC, 1.0, tempA, nFR);
2754  rnrm = 0.0;
2755  for ( i=0; i<nFR; ++i )
2756  if (rnrm < getAbs (tempA[i]))
2757  rnrm = getAbs (tempA[i]);
2758 
2759  if (!Delta_bC_isZero)
2760  {
2761  for ( i=0; i<nAC; ++i )
2762  {
2763  ii = AC_idx[i];
2764  if ( constraints.getStatus( ii ) == ST_LOWER )
2765  tempB[i] = delta_lbA[ii];
2766  else
2767  tempB[i] = delta_ubA[ii];
2768  }
2769  }
2770  else
2771  {
2772  for ( i=0; i<nAC; ++i )
2773  tempB[i] = 0.0;
2774  }
2775 
2776  A->times(constraints.getActive(), bounds.getFree(), 1, -1.0, delta_xFR, nFR, 1.0, tempB, nAC);
2777 
2778  A->times(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, tempB, nAC);
2779  for ( i=0; i<nAC; ++i )
2780  if (rnrm < getAbs (tempB[i]))
2781  rnrm = getAbs (tempB[i]);
2782 
2783  return SUCCESSFUL_RETURN;
2784 }
2785 
2786 returnValue SQProblemSchur::stepCalcDeltayFx(int_t nFR, int_t nFX, int_t nAC, int_t* FX_idx, const real_t* const delta_g, real_t* const delta_xFX, real_t* const delta_xFR, real_t* const delta_yAC, real_t* const delta_yFX)
2787 {
2788  int_t i;
2789  for( i=0; i<nFX; ++i )
2790  delta_yFX[i] = delta_g[FX_idx[i]];
2791 
2792  A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_yAC, nAC, 1.0, delta_yFX, nFX);
2793 
2794  if ( hessianType == HST_ZERO )
2795  {
2796  // TODO: if ( usingRegularisation( ) == BT_TRUE )
2797  for( i=0; i<nFX; ++i )
2798  delta_yFX[i] += options.epsRegularisation*delta_xFX[i];
2799  }
2800  else if ( hessianType == HST_IDENTITY )
2801  {
2802  for( i=0; i<nFX; ++i )
2803  delta_yFX[i] += delta_xFX[i];
2804  }
2805  else
2806  {
2807  H->times(bounds.getFixed(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, delta_yFX, nFX);
2808  H->times(bounds.getFixed(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, delta_yFX, nFX);
2809  }
2810  return SUCCESSFUL_RETURN;
2811 }
2812 
2813 returnValue SQProblemSchur::determineStepDirection( const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
2814  const real_t* const delta_lb, const real_t* const delta_ub,
2815  BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
2816  real_t* const delta_xFX, real_t* const delta_xFR,
2817  real_t* const delta_yAC, real_t* const delta_yFX
2818  )
2819 {
2820  returnValue retval = determineStepDirection2( delta_g, delta_lbA, delta_ubA, delta_lb, delta_ub,
2821  Delta_bC_isZero, Delta_bB_isZero, delta_xFX, delta_xFR,
2822  delta_yAC, delta_yFX
2823  );
2824 
2825  if ( retval == RET_QR_FACTORISATION_FAILED )
2826  {
2827  retval = resetSchurComplement( BT_FALSE );
2828  if (retval != SUCCESSFUL_RETURN)
2829  {
2830  MyPrintf( "In SQProblem::determineStepDirection, resetSchurComplement returns %d\n", retval);
2831  return THROWERROR( retval );
2832  }
2833  retval = determineStepDirection2( delta_g, delta_lbA, delta_ubA, delta_lb, delta_ub,
2834  Delta_bC_isZero, Delta_bB_isZero, delta_xFX, delta_xFR,
2835  delta_yAC, delta_yFX
2836  );
2837  }
2838  return retval;
2839 }
2840 
2841 /*
2842  * d e t e r m i n e S t e p D i r e c t i o n
2843  */
2844 returnValue SQProblemSchur::determineStepDirection2( const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
2845  const real_t* const delta_lb, const real_t* const delta_ub,
2846  BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
2847  real_t* const delta_xFX, real_t* const delta_xFR,
2848  real_t* const delta_yAC, real_t* const delta_yFX
2849  )
2850 {
2851  /* The linear system to be solved here is this:
2852 
2853  / H_FF H_FX A_CF^T 0 \ / delta_xFR \ / -delta_g_F \
2854  | H_XF H_XX A_CX^T I | | delta_xFX | | -delta_g_X |
2855  | A_CF A_CX 0 0 | | -delta_yAC | = | delta_bA | <-- active entries of delta_lbA and delta_ubA with corresponding sign
2856  \ 0 I 0 0 / \ -delta_yFX / \ delta_bX / <-- fixed entries of delta_lb and delta_ub with corresponding sign
2857 
2858  */
2859 
2860 
2861  int_t i, ii, r;
2862 
2863  returnValue retval;
2864 
2865  //int_t nV = getNV( );
2866  int_t nFR = getNFR( );
2867  int_t nFX = getNFX( );
2868  int_t nAC = getNAC( );
2869 
2870  int_t* FR_idx;
2871  int_t* FX_idx;
2872  int_t* AC_idx;
2873 
2874  bounds.getFree( )->getNumberArray( &FR_idx );
2875  bounds.getFixed( )->getNumberArray( &FX_idx );
2876  constraints.getActive( )->getNumberArray( &AC_idx );
2877 
2878 
2879  /* I) DETERMINE delta_xFX (this is exact, does not need refinement) */
2880  if ( Delta_bB_isZero == BT_FALSE )
2881  {
2882  for( i=0; i<nFX; ++i )
2883  {
2884  ii = FX_idx[i];
2885 
2886  if ( bounds.getStatus( ii ) == ST_LOWER )
2887  delta_xFX[i] = delta_lb[ii];
2888  else
2889  delta_xFX[i] = delta_ub[ii];
2890  }
2891  }
2892  else
2893  {
2894  for( i=0; i<nFX; ++i )
2895  delta_xFX[i] = 0.0;
2896  }
2897 
2898  if ( nFR+nAC>0 ) {
2899  real_t rhs_max = 0.0;
2900  retval = stepCalcRhs( nFR, nFX, nAC, FR_idx, FX_idx, AC_idx, rhs_max, delta_g, delta_lbA, delta_ubA,
2901  delta_lb, delta_ub, Delta_bC_isZero, Delta_bB_isZero, delta_xFX, delta_xFR,
2902  delta_yAC, delta_yFX );
2903 
2904  if (retval != SUCCESSFUL_RETURN)
2905  return retval;
2906  int_t nFRStart = boundsFreeStart.getLength();
2907  int_t nACStart = constraintsActiveStart.getLength();
2908 
2909  int_t* FR_iSort;
2910  int_t* AC_iSort;
2911  bounds.getFree( )->getISortArray( &FR_iSort );
2912  constraints.getActive( )->getISortArray( &AC_iSort );
2913 
2914  int_t* FR_idxStart;
2915  int_t* AC_idxStart;
2916  boundsFreeStart.getNumberArray( &FR_idxStart );
2917  constraintsActiveStart.getNumberArray( &AC_idxStart );
2918 
2919  int_t* FR_iSortStart;
2920  int_t* AC_iSortStart;
2921  boundsFreeStart.getISortArray( &FR_iSortStart );
2922  constraintsActiveStart.getISortArray( &AC_iSortStart );
2923 
2924  int_t dim = nFRStart + nACStart;
2925  real_t* rhs = new real_t[dim];
2926  real_t* sol = new real_t[dim];
2927 
2928  /* Iterative refinement loop for delta_xFR, delta_yAC */
2929  for ( r=0; r<=options.numRefinementSteps; ++r )
2930  {
2931  retval = stepCalcReorder(nFR, nAC, FR_idx, AC_idx, nFRStart, nACStart, FR_idxStart, AC_idxStart, FR_iSort, FR_iSortStart, AC_iSort, AC_iSortStart, rhs);
2932  if (retval != SUCCESSFUL_RETURN)
2933  return retval;
2934 
2935  retval = sparseSolver->solve(dim, rhs, sol);
2936 
2937  if (retval != SUCCESSFUL_RETURN)
2938  {
2939  MyPrintf( "sparseSolver->solve (first time) failed.\n");
2940  return THROWERROR(RET_MATRIX_FACTORISATION_FAILED); // TODO: Different return code
2941  }
2942 
2943  if ( nS > 0 )
2944  {
2945  retval = stepCalcBacksolveSchur( nFR, nFX, nAC, FR_idx, FX_idx, AC_idx, dim, rhs, sol );
2946  if (retval != SUCCESSFUL_RETURN)
2947  return retval;
2948  }
2949 
2950  /* Transfer Schur complement solution for the free variables to the correct places */
2951  retval = stepCalcReorder2(nFR, nAC, FR_idx, AC_idx, nFRStart, nACStart, FR_idxStart, AC_idxStart, FR_iSort, FR_iSortStart, AC_iSort, AC_iSortStart, sol, delta_xFR, delta_yAC);
2952  if (retval != SUCCESSFUL_RETURN)
2953  return retval;
2954 
2955  if ( r < options.numRefinementSteps ) // TODO: use "<" to avoid computation in last round
2956  {
2957  real_t rnrm;
2958  retval = stepCalcResid(nFR, nFX, nAC, FR_idx, FX_idx, AC_idx, Delta_bC_isZero, delta_xFX, delta_xFR, delta_yAC, delta_g, delta_lbA, delta_ubA, rnrm);
2959  if (retval != SUCCESSFUL_RETURN)
2960  return retval;
2961 
2962  /* early termination of residual norm small enough */
2963  if ( options.printLevel == PL_HIGH )
2964  MyPrintf( "In iterative refinement (iter %d) rnrm = %e and epsIterRef*rhs_max = %e.\n", r, rnrm, options.epsIterRef*rhs_max);
2965 
2966  if ( rnrm <= options.epsIterRef*rhs_max )
2967  break;
2968  }
2969 
2970  }
2971 
2972  delete [] sol;
2973  delete [] rhs;
2974  }
2975 
2976  /* IV) DETERMINE delta_yFX */
2977  if ( nFX > 0 )
2978  {
2979  retval = stepCalcDeltayFx(nFR, nFX, nAC, FX_idx, delta_g, delta_xFX, delta_xFR, delta_yAC, delta_yFX);
2980  if (retval != SUCCESSFUL_RETURN)
2981  return retval;
2982  }
2983 
2984  return SUCCESSFUL_RETURN;
2985 }
2986 
2987 returnValue SQProblemSchur::resetSchurComplement( BooleanType allowInertiaCorrection )
2988 {
2989  int_t j;
2990  int_t nFR = getNFR( );
2991  int_t nAC = getNAC( );
2992 
2993  if ( options.printLevel == PL_HIGH )
2994  MyPrintf( "Resetting Schur complement.\n");
2995 
2996  nS = 0;
2997  detS = 1.0;
2998  rcondS = 1.0;
2999  boundsFreeStart = *bounds.getFree();
3000  constraintsActiveStart = *constraints.getActive();
3001 
3002  if ( nSmax > 0 )
3003  M_jc[0] = 0;
3004 
3005  int_t dim = nFR+nAC;
3006  // Count the number of nonzeros
3007  int_t numNonzeros;
3008  switch ( hessianType )
3009  {
3010  case HST_ZERO:
3011  numNonzeros = 0;
3012  break;
3013 
3014  case HST_IDENTITY:
3015  numNonzeros = nFR;
3016  break;
3017 
3018  default:
3019  H->getSparseSubmatrix( bounds.getFree(), bounds.getFree(), 1, 1, numNonzeros, 0, 0, 0, BT_TRUE);
3020  break;
3021  }
3022  // TODO: For now, we regularize every time
3023  if (options.epsRegularisation > 0.0)
3024  numNonzeros += nFR;
3025 
3026  int_t numNonzerosA;
3027 
3028  if ( constraintProduct != 0 )
3029  {
3030  MyPrintf( "In SQProblemSchur::determineStepDirection, constraintProduct not yet implemented.\n");
3031  return THROWERROR(RET_NOT_YET_IMPLEMENTED);
3032  }
3033  A->getSparseSubmatrix( constraints.getActive(), bounds.getFree(), nFR+1, 1, numNonzerosA, 0, 0, 0, BT_FALSE);
3034  numNonzeros += numNonzerosA;
3035 
3036  // Get the values
3037  real_t* avals = new real_t[numNonzeros];
3038  int_t* irn = new int_t[numNonzeros];
3039  int_t* jcn = new int_t[numNonzeros];
3040  numNonzeros = 0;
3041  switch ( hessianType )
3042  {
3043  case HST_ZERO:
3044  break;
3045 
3046  case HST_IDENTITY:
3047  numNonzeros += nFR;
3048  for (j = 0; j<nFR; j++)
3049  {
3050  irn[j] = j+1;
3051  jcn[j] = j+1;
3052  avals[j] = 1.0;
3053  }
3054  break;
3055 
3056  default:
3057  H->getSparseSubmatrix( bounds.getFree(), bounds.getFree(), 1, 1, numNonzeros, irn, jcn, avals, BT_TRUE);
3058  break;
3059  }
3060 
3061  // For now, we regularize every time
3062  if (options.epsRegularisation > 0.0)
3063  {
3064  for (j = 0; j<nFR; j++)
3065  {
3066  irn[numNonzeros] = j+1;
3067  jcn[numNonzeros] = j+1;
3068  avals[numNonzeros++] = options.epsRegularisation;
3069  }
3070  }
3071 
3072  A->getSparseSubmatrix( constraints.getActive(), bounds.getFree(), nFR+1, 1, numNonzerosA, irn+numNonzeros, jcn+numNonzeros, avals+numNonzeros, BT_FALSE);
3073  numNonzeros += numNonzerosA;
3074 
3075  // Call the linear solver
3076  sparseSolver->reset();
3077  returnValue retval = sparseSolver->setMatrixData(dim, numNonzeros, irn, jcn, avals);
3078  delete [] jcn;
3079  delete [] irn;
3080  delete [] avals;
3081 
3082  if (retval != SUCCESSFUL_RETURN)
3083  return THROWERROR(RET_NO_SPARSE_SOLVER);
3084 
3085  // Factorize the matrix for later backsolves
3086  retval = sparseSolver->factorize();
3087  numFactorizations++;
3088 
3089  // If matrix is singular, add bounds/remove constraints according to zero pivots
3090  if (retval == RET_KKT_MATRIX_SINGULAR)
3091  {
3092  if( repairSingularWorkingSet( ) == SUCCESSFUL_RETURN )
3093  return resetSchurComplement( allowInertiaCorrection );
3094  else
3095  return RET_KKT_MATRIX_SINGULAR;
3096  }
3097 
3098  // If matrix has wrong inertia, add bounds until inertia is correct
3099  if (retval == SUCCESSFUL_RETURN && allowInertiaCorrection)
3100  {
3101  int_t neig = sparseSolver->getNegativeEigenvalues( );
3102  if( neig > getNAC( ) )
3103  {
3104  if ( options.printLevel == PL_HIGH )
3105  MyPrintf( "WARNING: After new factorization, reduced Hessian has %i negative eigenvalues, should be %i.\n", neig, getNAC( ) );
3106 
3107  retval = correctInertia();
3108  }
3109  }
3110 
3111  if (retval != SUCCESSFUL_RETURN)
3112  return THROWERROR(RET_MATRIX_FACTORISATION_FAILED);
3113 
3114  nS = 0;
3115 
3116  return SUCCESSFUL_RETURN;
3117 }
3118 
3119 returnValue SQProblemSchur::computeMTimes( real_t alpha, const real_t* const x_, real_t beta, real_t* const y_ )
3120 {
3121  if ( isEqual( alpha, -1.0 ) == BT_FALSE || isEqual( beta, 1.0 ) == BT_FALSE )
3122  return THROWERROR(RET_NOT_YET_IMPLEMENTED);
3123 
3124  int_t i, j;
3125 
3126  for ( j=0; j<nS; j++ )
3127  {
3128  const real_t xval = x_[j];
3129  for ( i=M_jc[j]; i<M_jc[j+1]; i++)
3130  y_[M_ir[i]] -= M_vals[i]*xval;
3131  }
3132 
3133  return SUCCESSFUL_RETURN;
3134 }
3135 
3136 returnValue SQProblemSchur::computeMTransTimes( real_t alpha, const real_t* const x_, real_t beta, real_t* const y_ )
3137 {
3138  if ( isEqual( alpha, 1.0 ) == BT_FALSE || ( isZero( beta ) == BT_FALSE && isEqual( beta, -1.0 ) == BT_FALSE ) )
3139  return THROWERROR(RET_NOT_YET_IMPLEMENTED);
3140 
3141  int_t i, j;
3142 
3143  if ( isZero( beta ) == BT_TRUE )
3144  {
3145  for ( j=0; j<nS; j++ )
3146  {
3147  y_[j] = 0.0;
3148  for ( i=M_jc[j]; i<M_jc[j+1]; i++)
3149  y_[j] += M_vals[i]*x_[M_ir[i]];
3150  }
3151  }
3152  else
3153  {
3154  for ( j=0; j<nS; j++ )
3155  {
3156  y_[j] = -y_[j];
3157  for ( i=M_jc[j]; i<M_jc[j+1]; i++)
3158  y_[j] += M_vals[i]*x_[M_ir[i]];
3159  }
3160  }
3161 
3162  return SUCCESSFUL_RETURN;
3163 }
3164 
3165 returnValue SQProblemSchur::addToSchurComplement( int_t number, SchurUpdateType update, int_t numNonzerosM, const sparse_int_t* Mpos, const real_t* const Mvals, int_t numNonzerosN, const sparse_int_t* Npos, const real_t* const Nvals, real_t N_diag )
3166 {
3167  int_t i;
3168 
3169  int_t nFRStart = boundsFreeStart.getLength();
3170  int_t nACStart = constraintsActiveStart.getLength();
3171 
3172  real_t* new_Scol = new real_t[nS];
3173 
3174  int_t dim = nFRStart + nACStart;
3175  real_t* rhs = new real_t[dim];
3176  real_t* sol = new real_t[dim];
3177 
3178  for ( i=0; i<dim; i++ )
3179  rhs[i] = 0.0;
3180 
3181  for ( i=0; i<numNonzerosM; i++ )
3182  rhs[Mpos[i]] = Mvals[i];
3183 
3184  returnValue retval = sparseSolver->solve(dim, rhs, sol);
3185  if (retval != SUCCESSFUL_RETURN)
3186  {
3187  MyPrintf( "sparseSolver->solve in SQProblemSchur::addToSchurComplement failed.\n");
3188  return THROWERROR(RET_MATRIX_FACTORISATION_FAILED); // TODO: Different return code
3189  }
3190 
3191  computeMTransTimes(1.0, sol, 0.0, new_Scol);
3192 
3193  /* Take care of off-diagonal elements in N. */
3194  for ( i=0; i<numNonzerosN; i++ )
3195  new_Scol[Npos[i]] -= Nvals[i];
3196 
3197  real_t sdiag = -N_diag;
3198  for ( i=0; i<numNonzerosM; i++ )
3199  sdiag += Mvals[i] * sol[Mpos[i]];
3200 
3201  /* Now augment S */
3202  for ( i=0; i<nS; i++)
3203  S[nS*nSmax + i] = new_Scol[i];
3204  for ( i=0; i<nS; i++)
3205  S[i*nSmax + nS] = new_Scol[i];
3206  S[nS*nSmax + nS] = sdiag;
3207 
3208  schurUpdateIndex[nS] = number;
3209  schurUpdate[nS] = update;
3210 
3211  /* Augment M matrix. */
3212  if ( M_physicallength < M_jc[nS] + numNonzerosM )
3213  {
3214  /* If necessary, allocate more memory for M. */
3215  int_t M_physicallength_new = getMax(2*M_physicallength, M_physicallength + 2*numNonzerosM);
3216  real_t* M_vals_new = new real_t[M_physicallength_new];
3217  sparse_int_t* M_ir_new = new sparse_int_t[M_physicallength_new];
3218  memcpy( M_vals_new, M_vals, ((unsigned int)(M_jc[nS]))*sizeof(real_t) );
3219  memcpy( M_ir_new, M_ir, ((unsigned int)(M_jc[nS]))*sizeof(sparse_int_t) );
3220  M_physicallength = M_physicallength_new;
3221  delete [] M_vals;
3222  delete [] M_ir;
3223  M_vals = M_vals_new;
3224  M_ir = M_ir_new;
3225  }
3226 
3227  for ( i=0; i<numNonzerosM; i++ )
3228  {
3229  M_vals[M_jc[nS] + i] = Mvals[i];
3230  M_ir[M_jc[nS] + i] = Mpos[i];
3231  }
3232  M_jc[nS+1] = M_jc[nS] + numNonzerosM;
3233 
3234  nS++;
3235 
3236  delete [] sol;
3237  delete [] rhs;
3238  delete [] new_Scol;
3239 
3240  if ( options.printLevel == PL_HIGH )
3241  MyPrintf( "added index %d with update type %d to Schur complement. nS = %d\n", number, update, nS);
3242 
3243  return SUCCESSFUL_RETURN;
3244 }
3245 
3246 
3247 returnValue SQProblemSchur::deleteFromSchurComplement( int_t idx, BooleanType allowUndo )
3248 {
3249  if ( options.printLevel == PL_HIGH )
3250  MyPrintf( "deleting entry %d with idx = %d and type %d from Schur complement.", idx, schurUpdateIndex[idx], schurUpdate[idx]);
3251 
3252  if ( idx != nS-1 )
3253  {
3254  real_t *temp_vals = NULL;
3255  int_t *temp_ir = NULL;
3256  int_t schurUpdateIndexTemp = -1;
3257  SchurUpdateType schurUpdateTemp = SUT_UNDEFINED;
3258 
3259  /* temporarily save the column of S to be deleted */
3260  if( allowUndo == BT_TRUE )
3261  {
3262  temp_vals = new real_t[nS];
3263  for ( int_t i=0; i<nS; i++ )
3264  temp_vals[i] = S[idx*nSmax + i];
3265 
3266  schurUpdateIndexTemp = schurUpdateIndex[idx];
3267  schurUpdateTemp = schurUpdate[idx];
3268  }
3269 
3270  /* Shift rows and columns >idx of S by one to the upper left */
3271  for ( int_t i=0; i<idx; i++ )
3272  for ( int_t j=idx+1; j<nS; j++ )
3273  S[i*nSmax + j-1] = S[i*nSmax + j];
3274  for ( int_t i=idx+1; i<nS; i++ )
3275  {
3276  for ( int_t j=0; j<idx; j++ )
3277  S[(i-1)*nSmax + j] = S[i*nSmax + j];
3278  for ( int_t j=idx+1; j<nS; j++ )
3279  S[(i-1)*nSmax + j-1] = S[i*nSmax + j];
3280  }
3281  for ( int_t i=idx+1; i<nS; i++ )
3282  {
3283  schurUpdateIndex[i-1] = schurUpdateIndex[i];
3284  schurUpdate[i-1] = schurUpdate[i];
3285  }
3286 
3287  /* Store deleted row/column in the last row/column of S, can retrieve it from there later */
3288  if( allowUndo == BT_TRUE )
3289  {
3290  for ( int_t i=0; i<nS; i++ )
3291  {
3292  S[(nS-1)*nSmax + i] = temp_vals[i];
3293  S[i*nSmax + (nS-1)] = temp_vals[i];
3294  }
3295  schurUpdateIndex[nS-1] = schurUpdateIndexTemp;
3296  schurUpdate[nS-1] = schurUpdateTemp;
3297  delete[] temp_vals;
3298  }
3299 
3300  /* temporarily save the (sparse) column of M to be deleted */
3301  int_t numEntries = M_jc[idx+1] - M_jc[idx];
3302  if( allowUndo == BT_TRUE )
3303  {
3304  temp_ir = new int_t[numEntries];
3305  temp_vals = new real_t[numEntries];
3306 
3307  for ( int_t i=M_jc[idx]; i<M_jc[idx+1]; i++ )
3308  {
3309  temp_ir[i-M_jc[idx]] = M_ir[i];
3310  temp_vals[i-M_jc[idx]] = M_vals[i];
3311  }
3312  }
3313 
3314  /* Shift all columns >idx one to the left */
3315  for ( int_t i=M_jc[idx+1]; i<M_jc[nS]; i++ )
3316  {
3317  M_ir[i-numEntries] = M_ir[i];
3318  M_vals[i-numEntries] = M_vals[i];
3319  }
3320  for ( int_t i=idx; i<nS; i++ )
3321  M_jc[i] = M_jc[i+1] - numEntries;
3322 
3323  /* Store deleted column of M in the last column, can retrieve it from there later */
3324  if( allowUndo == BT_TRUE )
3325  {
3326  for ( int_t i=M_jc[nS-1]; i<M_jc[nS]; i++ )
3327  {
3328  M_ir[i] = temp_ir[i-M_jc[nS-1]];
3329  M_vals[i] = temp_vals[i-M_jc[nS-1]];
3330  }
3331 
3332  delete[] temp_ir;
3333  delete[] temp_vals;
3334  }
3335  }
3336 
3337  nS--;
3338 
3339  if ( options.printLevel == PL_HIGH )
3340  MyPrintf( " nS = %d\n", nS);
3341 
3342  return SUCCESSFUL_RETURN;
3343 }
3344 
3345 
3346 returnValue SQProblemSchur::undoDeleteFromSchurComplement( int_t idx )
3347 {
3348  if ( options.printLevel == PL_HIGH )
3349  MyPrintf( "undo deletion of entry %d with idx = %d and type %d from Schur complement. nS = %i\n", idx, schurUpdateIndex[nS-1], schurUpdate[nS-1],nS+1);
3350 
3351  if ( idx != nS )
3352  {
3353  real_t *temp_vals;
3354  int_t *temp_ir;
3355  int_t schurUpdateIndexTemp = -1;
3356  SchurUpdateType schurUpdateTemp = SUT_UNDEFINED;
3357 
3358  /* temporarily save the last column of S */
3359  temp_vals = new real_t[nS+1];
3360  for ( int_t i=0; i<nS+1; i++ )
3361  temp_vals[i] = S[i+nS*nSmax];
3362 
3363  schurUpdateIndexTemp = schurUpdateIndex[nS];
3364  schurUpdateTemp = schurUpdate[nS];
3365 
3366  /* Shift rows and columns =>idx of S by one to the lower right */
3367  for ( int_t i=idx-1; i>-1; i-- )
3368  for ( int_t j=nS-1; j>idx-1; j-- )
3369  S[(j+1)+i*nSmax] = S[j+i*nSmax];
3370  for ( int_t i=nS-1; i>idx-1; i-- )
3371  {
3372  for ( int_t j=idx-1; j>-1; j-- )
3373  S[j+(i+1)*nSmax] = S[j+i*nSmax];
3374  for ( int_t j=nS-1; j>idx-1; j-- )
3375  S[(j+1)+(i+1)*nSmax] = S[j+i*nSmax];
3376  }
3377  for ( int_t i=nS-1; i>idx-1; i-- )
3378  {
3379  schurUpdateIndex[i+1] = schurUpdateIndex[i];
3380  schurUpdate[i+1] = schurUpdate[i];
3381  }
3382 
3383  /* Insert stored row/column of S at position idx */
3384  for ( int_t i=0; i<nS+1; i++ )
3385  {
3386  S[idx*nSmax + i] = temp_vals[i];
3387  S[i*nSmax + idx] = temp_vals[i];
3388  }
3389  schurUpdateIndex[idx] = schurUpdateIndexTemp;
3390  schurUpdate[idx] = schurUpdateTemp;
3391  delete[] temp_vals;
3392 
3393  /* temporarily save the last (sparse) column of M */
3394  int_t numEntries = M_jc[nS+1] - M_jc[nS];
3395  temp_ir = new int_t[numEntries];
3396  temp_vals = new real_t[numEntries];
3397  for ( int_t i=M_jc[nS]; i<M_jc[nS+1]; i++ )
3398  {
3399  temp_ir[i-M_jc[nS]] = M_ir[i];
3400  temp_vals[i-M_jc[nS]] = M_vals[i];
3401  }
3402 
3403  /* Shift all columns =>idx one to the right */
3404  for ( int_t i=M_jc[nS]-1; i>M_jc[idx]-1; i-- )
3405  {
3406  M_ir[i+numEntries] = M_ir[i];
3407  M_vals[i+numEntries] = M_vals[i];
3408  }
3409  for ( int_t i=nS; i>idx-1; i-- )
3410  M_jc[i+1] = M_jc[i] + numEntries;
3411 
3412  /* Insert stored column of M at position idx */
3413  for ( int_t i=M_jc[idx]; i<M_jc[idx+1]; i++ )
3414  {
3415  M_ir[i] = temp_ir[i-M_jc[idx]];
3416  M_vals[i] = temp_vals[i-M_jc[idx]];
3417  }
3418 
3419  delete[] temp_ir;
3420  delete[] temp_vals;
3421  }
3422 
3423  nS++;
3424 
3425  if ( options.printLevel == PL_HIGH )
3426  MyPrintf( " nS = %d\n", nS);
3427 
3428  return SUCCESSFUL_RETURN;
3429 }
3430 
3431 returnValue SQProblemSchur::correctInertia( )
3432 {
3433  SubjectToStatus B_status;
3434  real_t oldDetS;
3435  int_t nFR = getNFR( );
3436  int_t k, number, neig, nAdded;
3437  int_t *freeBoundIdx = new int_t[nFR];
3438  int_t *numberarray;
3439 
3440  /* method may only be called after refactorization or if one bound/constraint
3441  * has been added, i.e. when a bound has flipped after refactorization */
3442  if( nS != 0 && nS != 1 )
3443  return THROWERROR( RET_INERTIA_CORRECTION_FAILED );
3444  neig = sparseSolver->getNegativeEigenvalues( );
3445 
3446  /* if a bound flipped, check if it did in fact remove a negative eigenvalue */
3447  if( nS == 1 && detS < 0 )
3448  neig--;
3449 
3450  /* if this method is triggered after flipping bounds, inertia is now probably correct */
3451  if( neig == getNAC( ) )
3452  return SUCCESSFUL_RETURN;
3453 
3454  /* get bound numbers in the order in which they are in the non-basis */
3455  bounds.getFree()->getNumberArray( &numberarray );
3456  for( k=0; k<nFR; k++ )
3457  freeBoundIdx[k] = numberarray[k];
3458 
3459  k = 0;
3460  nAdded = getNFR( );
3461  while ( neig > getNAC( ) && k < nFR )
3462  {
3463  oldDetS = detS;
3464 
3465  /* If it's linearly independent, fix the next free variable at the nearest bound */
3466  number = freeBoundIdx[k];
3467  if( addBound_checkLI( number ) == RET_LINEARLY_INDEPENDENT )
3468  {
3469  /* This is just heuristics: we need the bound which gives correct multiplier sign */
3470  if ( x[number] - lb[number] < ub[number] - x[number] )
3471  B_status = ST_LOWER;
3472  else
3473  B_status = ST_UPPER;
3474 
3475  /* Update Schur complement */
3476  if( addBound( number, B_status, BT_TRUE, BT_FALSE ) != SUCCESSFUL_RETURN )
3477  {
3478  if ( options.printLevel == PL_HIGH )
3479  MyPrintf("In correctInertia: Adding bound[%i] = %i failed!\n", k, number );
3480  return THROWERROR( RET_INERTIA_CORRECTION_FAILED );
3481  }
3482 
3483  /* Adjust bounds */
3484  if ( B_status == ST_LOWER )
3485  lb[number] = x[number];
3486  else
3487  ub[number] = x[number];
3488  }
3489  else
3490  {
3491  if ( options.printLevel == PL_HIGH )
3492  MyPrintf("bound[%i] = %i is linearly dependent. Do not add.\n", k, number );
3493  k++;
3494  continue;
3495  }
3496 
3497  /* Case 1: Schur complement has been reset, check inertia of new factorization */
3498  if( nS == 0 )
3499  neig = sparseSolver->getNegativeEigenvalues( );
3500  /* Case 2: Schur complement has grown, check if determinant changed sign */
3501  else if( oldDetS * detS < 0 )
3502  neig--;
3503  /* NB: Case 3: (Schur complement has shrunk) cannot happen here:
3504  * This method is called after a factorization reset or after ONE bound has been added */
3505 
3506  k++;
3507  }
3508  nAdded -= getNFR( );
3509 
3510  delete[] freeBoundIdx;
3511 
3512  /* if there are still too many negative eigenvalues, exit */
3513  if( neig > getNAC( ) )
3514  {
3515  if ( options.printLevel == PL_HIGH )
3516  MyPrintf( "Added %i bounds but KKT matrix still has %i negative eigenvalues, should be %i.\n", nAdded, neig, getNAC( ) );
3517  return THROWERROR( RET_INERTIA_CORRECTION_FAILED );
3518  }
3519  else
3520  {
3521  if ( options.printLevel == PL_HIGH )
3522  MyPrintf( "After adding %i bounds, reduced Hessian has correct inertia.\n", nAdded, neig );
3523  return SUCCESSFUL_RETURN;
3524  }
3525 }
3526 
3527 
3528 returnValue SQProblemSchur::repairSingularWorkingSet( )
3529 {
3530  int_t k, number;
3531  SubjectToStatus B_status;
3532  int_t rank = sparseSolver->getRank( );
3533  int_t nFR = getNFR( );
3534  int_t defect = nFR + getNAC( ) - rank;
3535 
3536  /* Rank detection not supported by linear solver */
3537  if ( rank < 0 )
3538  return RET_KKT_MATRIX_SINGULAR;
3539 
3540  /* Consistency check */
3541  if ( defect <= 0 )
3542  return RET_UNKNOWN_BUG;
3543 
3544  /* Determine zero pivots */
3545  int_t *zeroPivots = new int_t[defect];
3546  sparseSolver->getZeroPivots( zeroPivots );
3547 
3548  /* Determination of zero pivots not supported by linear solver */
3549  if ( zeroPivots == 0 )
3550  return RET_KKT_MATRIX_SINGULAR;
3551 
3552  /* We assume implicitly that pivots are sorted in ascending order */
3554  /* Remove the one with the highest index first so not to mess up index lists */
3555  int_t bndsAdded = 0;
3556  for ( k=defect-1; k>-1; k-- )
3557  {
3558  /* Zero curvature in the Hessian: add a bound */
3559  if ( zeroPivots[k] < nFR )
3560  {
3561  number = bounds.getFree()->getNumber( zeroPivots[k] );
3562 
3563  if ( options.printLevel == PL_HIGH )
3564  MyPrintf( "WARNING: KKT matrix singular! Add bound %i before refactorization.\n", number);
3565 
3566  /* This is just heuristics: we need the bound which gives correct multiplier sign */
3567  if ( x[number] - lb[number] < ub[number] - x[number] )
3568  B_status = ST_LOWER;
3569  else
3570  B_status = ST_UPPER;
3571 
3572  /* Here we do not need to update the Schur complement because KKT matrix is factorized afterwards */
3573  if ( bounds.moveFreeToFixed( number, B_status ) != SUCCESSFUL_RETURN )
3574  return RET_ADDBOUND_FAILED;
3575 
3576  /* Adjust bounds */
3577  if ( B_status == ST_LOWER )
3578  lb[number] = x[number];
3579  else
3580  ub[number] = x[number];
3581 
3582  bndsAdded++;
3583  }
3584  /* Linearly dependent row in the Jacobian: remove a constraint */
3585  else
3586  {
3587  number = constraints.getActive()->getNumber( zeroPivots[k]-nFR );
3588  if ( options.printLevel == PL_HIGH )
3589  MyPrintf( "WARNING: KKT matrix singular! Removing constraint %i before refactorization.\n", number);
3590 
3591  if ( constraints.moveActiveToInactive( number ) != SUCCESSFUL_RETURN )
3592  return RET_REMOVECONSTRAINT_FAILED;
3593 
3594  // AW: If this is an equality constraint, it is now inactive and
3595  // will not be considered in the step computation which leads to
3596  // violation of that constraint in the future. Here, I try to
3597  // fix this by simply making this constraint no longer an
3598  // equality.
3599  // TODO: This is probably also necessary for bound constraints
3600  if ( constraints.getType(number) == ST_EQUALITY )
3601  {
3602  if ( options.printLevel == PL_HIGH )
3603  MyPrintf( "WARNING: Making this constraint no longer an equality.\n");
3604  constraints.setType( number, ST_BOUNDED );
3605  }
3606 
3607  /* Adjust dual variable */
3608  y[number] = 0.0;
3609  }
3610  }
3611 
3612  if ( options.printLevel == PL_HIGH )
3613  MyPrintf( "WARNING: KKT matrix singular! Removed %i constraints and added %i bounds before refactorization.\n",
3614  defect-bndsAdded, bndsAdded );
3615 
3616  delete[] zeroPivots;
3617 
3618  return SUCCESSFUL_RETURN;
3619 }
3620 
3621 END_NAMESPACE_QPOASES
3622 
3623 
3624 /*
3625  * end of file
3626  */
RET_REMOVEBOUND_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:146
getMax
int getMax(int x, int y)
SparseSolver::factorize
virtual returnValue factorize()=0
RET_LINEARLY_DEPENDENT
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:153
N
#define N
Definition: crane_kul_mhe_test.cpp:44
QProblem::status
QProblemStatus status
Definition: QProblem.h:82
SQProblemSchur::stepCalcBacksolveSchur
returnValue stepCalcBacksolveSchur(int_t nFR, int_t nFX, int_t nAC, int_t *FR_idx, int_t *FX_idx, int_t *AC_idx, int_t dim, real_t *rhs, real_t *sol)
Definition: SQProblemSchur.cpp:2579
M
#define M
Definition: rocket_with_templates_ocp.hpp:43
SQProblemSchur::addBound
virtual returnValue addBound(int_t number, SubjectToStatus B_status, BooleanType updateCholesky, BooleanType ensureLI=BT_TRUE)
Definition: SQProblemSchur.cpp:1120
SubjectTo::setType
returnValue setType(int i, SubjectToType value)
RET_INERTIA_CORRECTION_FAILED
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:223
RET_MATRIX_FACTORISATION_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:166
RET_UNKNOWN_BUG
Definition: acado_types.hpp:845
ST_LOWER
#define ST_LOWER
Definition: qpOASES_wrapper.h:73
Bounds::getFree
Indexlist * getFree()
SQProblemSchur::setupTQfactorisation
virtual returnValue setupTQfactorisation()
Definition: SQProblemSchur.cpp:644
getAbs
real_t getAbs(real_t x)
Constraints::getNC
int getNC() const
SQProblemSchur::deleteFromSchurComplement
returnValue deleteFromSchurComplement(int_t idx, BooleanType allowUndo=BT_FALSE)
Definition: SQProblemSchur.cpp:3247
RET_RESET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:99
SparseSolver::setMatrixData
virtual returnValue setMatrixData(int_t dim, int_t numNonzeros, const int_t *const airn, const int_t *const acjn, const real_t *const avals)=0
Constraints::moveInactiveToActive
returnValue moveInactiveToActive(int _number, SubjectToStatus _status)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/SRC/Constraints.cpp:227
__LINE__
#define __LINE__
Definition: acado_message_handling.hpp:98
TRTRS
#define TRTRS
Definition: SQProblemSchur.hpp:63
isZero
BooleanType isZero(real_t x, real_t TOL=ZERO)
SQProblemSchur::backsolveT
virtual returnValue backsolveT(const real_t *const b, BooleanType transposed, real_t *const a) const
Definition: SQProblemSchur.cpp:2169
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
RET_MOVING_BOUND_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:94
QProblemB::getNFX
int getNFX()
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
QProblemB::computeGivens
void computeGivens(real_t xold, real_t yold, real_t &xnew, real_t &ynew, real_t &c, real_t &s) const
SQProblemSchur::undoDeleteFromSchurComplement
returnValue undoDeleteFromSchurComplement(int_t idx)
Definition: SQProblemSchur.cpp:3346
SQProblem
Implements the online active set strategy for QPs with varying matrices.
Definition: qpOASES-3.0beta/include/qpOASES/SQProblem.hpp:59
TRCON
#define TRCON
Definition: SQProblemSchur.hpp:65
THROWINFO
#define THROWINFO(retval)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:397
SUT_VarFreed
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:282
MessageHandling::throwInfo
returnValue throwInfo(returnValue Inumber, const char *additionaltext, const char *functionname, const char *filename, const unsigned long linenumber, VisibilityStatus localVisibilityStatus)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/SRC/MessageHandling.cpp:315
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
SQProblemSchur::M_jc
sparse_int_t * M_jc
Definition: SQProblemSchur.hpp:459
RET_INDEXLIST_CORRUPTED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:83
DenseMatrix::getSparseSubmatrix
virtual returnValue getSparseSubmatrix(int_t irowsLength, const int_t *const irowsNumber, int_t icolsLength, const int_t *const icolsNumber, int_t rowoffset, int_t coloffset, int_t &numNonzeros, int_t *irn, int_t *jcn, real_t *avals, BooleanType only_lower_triangular=BT_FALSE) const
Definition: qpOASES-3.2.0/src/Matrices.cpp:214
QProblem::hessianType
HessianType hessianType
Definition: QProblem.h:87
QProblem::ensureNonzeroCurvature
returnValue ensureNonzeroCurvature(BooleanType removeBoundNotConstraint, int remIdx, BooleanType &exchangeHappened, BooleanType &addBoundNotConstraint, int &addIdx, SubjectToStatus &addStatus)
Definition: external_packages/qpOASES-3.0beta/src/QProblem.cpp:4306
SQProblemSchur::addConstraint_checkLI
virtual returnValue addConstraint_checkLI(int_t number)
Definition: SQProblemSchur.cpp:839
SQProblemSchur::computeProjectedCholesky
virtual returnValue computeProjectedCholesky()
Definition: SQProblemSchur.cpp:626
SQProblemSchur::resetSchurComplement
returnValue resetSchurComplement(BooleanType allowInertiaCorrection)
Definition: SQProblemSchur.cpp:2987
QProblemB::getNV
int getNV() const
QProblemB::setH
returnValue setH(const real_t *const H_new)
RET_ADDCONSTRAINT_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:141
SQProblemSchur::computeMTimes
returnValue computeMTimes(real_t alpha, const real_t *const x, real_t beta, real_t *const y)
Definition: SQProblemSchur.cpp:3119
SQProblemSchur::sparseSolver
SparseSolver * sparseSolver
Definition: SQProblemSchur.hpp:441
THROWERROR
#define THROWERROR(retval)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:391
SQProblem::operator=
SQProblem & operator=(const SQProblem &rhs)
Definition: qpOASES-3.0beta/src/SQProblem.cpp:83
Constraints::getInactive
Indexlist * getInactive()
QProblemB::setInfeasibilityFlag
returnValue setInfeasibilityFlag(returnValue returnvalue)
Definition: external_packages/qpOASES-3.0beta/src/QProblemB.cpp:2006
HessianType
HessianType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:116
QPS_SOLVED
Definition: acado_types.hpp:674
__FUNCTION__
#define __FUNCTION__
Definition: acado_message_handling.hpp:90
HST_IDENTITY
#define HST_IDENTITY
Definition: qpOASES_wrapper.h:65
SQProblemSchur::schurUpdateIndex
int_t * schurUpdateIndex
Definition: SQProblemSchur.hpp:453
WORK
#define WORK(I)
RET_REMOVECONSTRAINT_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:145
sparse_int_t
int_t sparse_int_t
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:189
SQProblemSchur::determineStepDirection2
virtual returnValue determineStepDirection2(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: SQProblemSchur.cpp:2844
SQProblemSchur::calcDetSchur
real_t calcDetSchur(int_t idxDel)
Definition: SQProblemSchur.cpp:2196
SQProblemSchur::M_ir
sparse_int_t * M_ir
Definition: SQProblemSchur.hpp:458
RET_LI_RESOLVED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:155
SQProblemSchur::reset
virtual returnValue reset()
Definition: SQProblemSchur.cpp:196
ST_UNDEFINED
#define ST_UNDEFINED
Definition: qpOASES_wrapper.h:78
RET_KKT_MATRIX_SINGULAR
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:221
SUT_ConRemoved
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:284
SQProblemSchur::addToSchurComplement
returnValue addToSchurComplement(int_t number, SchurUpdateType update, int_t numNonzerosM, const sparse_int_t *M_pos, const real_t *const M_vals, int_t numNonzerosN, const sparse_int_t *Npos, const real_t *const Nvals, real_t N_diag)
Definition: SQProblemSchur.cpp:3165
SparseSolver::getRank
virtual int_t getRank()
Definition: SparseSolver.cpp:115
SUT_ConAdded
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:283
ST_INACTIVE
#define ST_INACTIVE
Definition: qpOASES_wrapper.h:74
SQProblemSchur::correctInertia
returnValue correctInertia()
Definition: SQProblemSchur.cpp:3431
int_t
int int_t
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:174
SQProblemSchur::addBound_ensureLI
virtual returnValue addBound_ensureLI(int_t number, SubjectToStatus B_status)
Definition: SQProblemSchur.cpp:1362
QProblem::getNAC
int getNAC()
RET_SETUP_WORKINGSET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:108
SQProblemSchur::operator=
virtual SQProblemSchur & operator=(const SQProblemSchur &rhs)
Definition: SQProblemSchur.cpp:181
MyPrintf
void MyPrintf(const char *pformat,...)
Definition: SQProblemSchur.cpp:42
SQProblemSchur::addBound_checkLI
virtual returnValue addBound_checkLI(int_t number)
Definition: SQProblemSchur.cpp:1255
PL_HIGH
#define PL_HIGH
Definition: qpOASES_wrapper.h:86
SQProblemSchur::updateSchurQR
returnValue updateSchurQR(int_t idxDel)
Definition: SQProblemSchur.cpp:2289
QProblemB::usingRegularisation
BooleanType usingRegularisation() const
SparseSolver::getNegativeEigenvalues
virtual int_t getNegativeEigenvalues()
Definition: SparseSolver.cpp:107
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
SubjectTo::getStatus
SubjectToStatus getStatus(int i) const
QProblem::bounds
Bounds bounds
Definition: QProblem.h:72
QProblemB::performRatioTest
returnValue performRatioTest(int nIdx, const int *const idxList, const SubjectTo *const subjectTo, const real_t *const num, const real_t *const den, real_t epsNum, real_t epsDen, real_t &t, int &BC_idx) const
Definition: external_packages/qpOASES-3.0beta/src/QProblemB.cpp:2081
SQProblemSchur::constraintsActiveStart
Indexlist constraintsActiveStart
Definition: SQProblemSchur.hpp:462
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
SQProblemSchur::~SQProblemSchur
virtual ~SQProblemSchur()
Definition: SQProblemSchur.cpp:170
SUT_VarFixed
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:281
SQProblemSchur::stepCalcRhs
returnValue stepCalcRhs(int_t nFR, int_t nFX, int_t nAC, int_t *FR_idx, int_t *FX_idx, int_t *AC_idx, real_t &rhs_max, 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: SQProblemSchur.cpp:2466
nu
const int nu
Definition: rocket_with_templates_ocp.hpp:38
QProblem::setupAuxiliaryQPbounds
returnValue setupAuxiliaryQPbounds(const Bounds *const auxiliaryBounds, const Constraints *const auxiliaryConstraints, BooleanType useRelaxation)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/SRC/QProblem.cpp:1198
RET_ENSURELI_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:156
SQProblemSchur::repairSingularWorkingSet
returnValue repairSingularWorkingSet()
Definition: SQProblemSchur.cpp:3528
SQProblemSchur::stepCalcResid
returnValue stepCalcResid(int_t nFR, int_t nFX, int_t nAC, int_t *FR_idx, int_t *FX_idx, int_t *AC_idx, BooleanType Delta_bC_isZero, real_t *const delta_xFX, real_t *const delta_xFR, real_t *const delta_yAC, const real_t *const delta_g, const real_t *const delta_lbA, const real_t *const delta_ubA, real_t &rnrm)
Definition: SQProblemSchur.cpp:2724
SQProblemSchur::addConstraint_ensureLI
virtual returnValue addConstraint_ensureLI(int_t number, SubjectToStatus C_status)
Definition: SQProblemSchur.cpp:952
QProblem::x
real_t x[NVMAX]
Definition: QProblem.h:77
Indexlist::getLength
int getLength()
SQProblemSchur::addConstraint
virtual returnValue addConstraint(int_t number, SubjectToStatus C_status, BooleanType updateCholesky, BooleanType ensureLI=BT_TRUE)
Definition: SQProblemSchur.cpp:653
QProblem::tabularOutput
TabularOutput tabularOutput
Definition: QProblem.h:99
SQProblemSchur::schurUpdate
SchurUpdateType * schurUpdate
Definition: SQProblemSchur.hpp:454
QProblemB::getNFR
int getNFR()
QPS_PERFORMINGHOMOTOPY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:79
SQProblemSchur::addConstraint_checkLISchur
returnValue addConstraint_checkLISchur(int_t number, real_t *const xiC, real_t *const xiX)
Definition: SQProblemSchur.cpp:860
Matrix
Abstract base class for interfacing tailored matrix-vector operations.
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:112
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
SubjectToStatus
SubjectToStatus
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:97
RET_NO_HESSIAN_SPECIFIED
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:171
SQProblemSchur::setupAuxiliaryWorkingSet
virtual returnValue setupAuxiliaryWorkingSet(const Bounds *const auxiliaryBounds, const Constraints *const auxiliaryConstraints, BooleanType setupAfresh)
Definition: SQProblemSchur.cpp:517
VS_VISIBLE
Definition: acado_message_handling.hpp:53
RET_ALL_BOUNDS_ACTIVE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:161
RET_ALL_CONSTRAINTS_ACTIVE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:152
SQProblemSchur::determineStepDirection
virtual returnValue determineStepDirection(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: SQProblemSchur.cpp:2813
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
RET_LINEARLY_INDEPENDENT
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:154
END_NAMESPACE_QPOASES
#define END_NAMESPACE_QPOASES
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/Types.hpp:120
QProblem::getNC
int getNC() const
SQProblemSchur::stepCalcReorder2
returnValue stepCalcReorder2(int_t nFR, int_t nAC, int_t *FR_idx, int_t *AC_idx, int_t nFRStart, int_t nACStart, int_t *FR_idxStart, int_t *AC_idxStart, int_t *FR_iSort, int_t *FR_iSortStart, int_t *AC_iSort, int_t *AC_iSortStart, real_t *sol, real_t *const delta_xFR, real_t *const delta_yAC)
Definition: SQProblemSchur.cpp:2683
QProblem::y
real_t y[NVMAX+NCMAX]
Definition: QProblem.h:78
RET_ADDBOUND_FAILED_INFEASIBILITY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:144
RET_CONSTRAINT_ALREADY_ACTIVE
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:151
SubjectTo::getType
SubjectToType getType(int i) const
SQProblemSchur::removeConstraint
virtual returnValue removeConstraint(int_t number, BooleanType updateCholesky, BooleanType allowFlipping=BT_FALSE, BooleanType ensureNZC=BT_FALSE)
Definition: SQProblemSchur.cpp:1528
SUT_UNDEFINED
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:285
QProblemB::getStatus
QProblemStatus getStatus() const
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
SQProblemSchur::computeMTransTimes
returnValue computeMTransTimes(real_t alpha, const real_t *const x, real_t beta, real_t *const y)
Definition: SQProblemSchur.cpp:3136
RET_SETUP_AUXILIARYQP_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:109
SQProblemSchur::copy
returnValue copy(const SQProblemSchur &rhs)
Definition: SQProblemSchur.cpp:240
RET_REMOVE_FROM_ACTIVESET
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:147
QProblem::delta_xFR_TMP
real_t delta_xFR_TMP[NVMAX]
Definition: QProblem.h:92
SchurUpdateType
SchurUpdateType
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/Types.hpp:279
HST_ZERO
#define HST_ZERO
Definition: qpOASES_wrapper.h:64
Bounds::getNUV
int getNUV() const
DenseMatrix::times
returnValue times(int xN, real_t alpha, const real_t *x, int xLD, real_t beta, real_t *y, int yLD) const
Definition: qpOASES-3.0beta/src/Matrices.cpp:151
HST_SEMIDEF
#define HST_SEMIDEF
Definition: qpOASES_wrapper.h:68
SQProblemSchur::setupAuxiliaryQP
virtual returnValue setupAuxiliaryQP(SymmetricMatrix *H_new, Matrix *A_new, const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new)
Definition: SQProblemSchur.cpp:317
SparseSolver::reset
virtual returnValue reset()
Definition: SparseSolver.cpp:99
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
HST_UNKNOWN
#define HST_UNKNOWN
Definition: qpOASES_wrapper.h:70
SQProblemSchur::boundsFreeStart
Indexlist boundsFreeStart
Definition: SQProblemSchur.hpp:461
QProblem::R
real_t R[NVMAX *NVMAX]
Definition: QProblem.h:74
Constraints::getActive
Indexlist * getActive()
SQProblemSchur::backsolveR
virtual returnValue backsolveR(const real_t *const b, BooleanType transposed, real_t *const a) const
Definition: SQProblemSchur.cpp:2178
SQProblemSchur::removeBound
virtual returnValue removeBound(int_t number, BooleanType updateCholesky, BooleanType allowFlipping=BT_FALSE, BooleanType ensureNZC=BT_FALSE)
Definition: SQProblemSchur.cpp:1803
SubjectTo::setStatus
returnValue setStatus(int i, SubjectToStatus value)
SQProblemSchur::computeInitialCholesky
virtual returnValue computeInitialCholesky()
Definition: SQProblemSchur.cpp:635
QPS_PREPARINGAUXILIARYQP
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:75
QProblem::H
DenseMatrix * H
Definition: QProblem.h:65
SQProblemSchur::addBound_checkLISchur
returnValue addBound_checkLISchur(int_t number, real_t *const xiC, real_t *const xiX)
Definition: SQProblemSchur.cpp:1275
QProblem::ub
real_t ub[NVMAX]
Definition: QProblem.h:70
QProblem::lb
real_t lb[NVMAX]
Definition: QProblem.h:69
__FILE__
#define __FILE__
Definition: acado_message_handling.hpp:94
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
RET_NO_SPARSE_SOLVER
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:224
Constraints::getNUC
int getNUC() const
ST_EQUALITY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:91
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
SQProblemSchur
Implements the online active set strategy for QPs with varying, sparse matrices.
Definition: SQProblemSchur.hpp:115
QProblem::dropInfeasibles
returnValue dropInfeasibles(int_t BC_number, SubjectToStatus BC_status, BooleanType BC_isBound, real_t *xiB, real_t *xiC)
Definition: external_packages/qpOASES-3.2.0/src/QProblem.cpp:6234
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
SQProblemSchur::clear
returnValue clear()
Definition: SQProblemSchur.cpp:216
RET_NOT_YET_IMPLEMENTED
Definition: acado_types.hpp:847
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
RET_ENSURELI_FAILED_NOINDEX
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:158
QProblemB::applyGivens
void applyGivens(real_t c, real_t s, real_t xold, real_t yold, real_t &xnew, real_t &ynew) const
Indexlist::getNumber
int getNumber(int physicalindex) const
SparseSolver::solve
virtual returnValue solve(double *b)=0
RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED
Definition: external_packages/qpOASES-3.0beta/include/qpOASES/MessageHandling.hpp:185
SQProblemSchur::stepCalcDeltayFx
returnValue stepCalcDeltayFx(int_t nFR, int_t nFX, int_t nAC, int_t *FX_idx, const real_t *const delta_g, real_t *const delta_xFX, real_t *const delta_xFR, real_t *const delta_yAC, real_t *const delta_yFX)
Definition: SQProblemSchur.cpp:2786
getGlobalMessageHandler
MessageHandling * getGlobalMessageHandler()
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/SRC/MessageHandling.cpp:515
QProblem::setA
returnValue setA(const real_t *const A_new)
RET_REMOVE_FROM_ACTIVESET_FAILED
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:149
Bounds::getFixed
Indexlist * getFixed()
RET_QR_FACTORISATION_FAILED
Definition: external_packages/qpOASES-3.2.0/include/qpOASES/MessageHandling.hpp:222
isEqual
BooleanType isEqual(real_t x, real_t y, real_t TOL=ZERO)
SQProblemSchur::backsolveSchurQR
returnValue backsolveSchurQR(int_t dimS, const real_t *const rhs, int_t dimRhs, real_t *const sol)
Definition: SQProblemSchur.cpp:2428
SQProblemSchur::stepCalcReorder
returnValue stepCalcReorder(int_t nFR, int_t nAC, int_t *FR_idx, int_t *AC_idx, int_t nFRStart, int_t nACStart, int_t *FR_idxStart, int_t *AC_idxStart, int_t *FR_iSort, int_t *FR_iSortStart, int_t *AC_iSort, int_t *AC_iSortStart, real_t *rhs)
Definition: SQProblemSchur.cpp:2533
Indexlist::getISortArray
returnValue getISortArray(int_t **const iSortArray) const
Definition: external_packages/qpOASES-3.2.0/src/Indexlist.cpp:146
RET_ADDCONSTRAINT_FAILED_INFEASIBILITY
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/MessageHandling.hpp:142
Bounds::moveFreeToFixed
returnValue moveFreeToFixed(int _number, SubjectToStatus _status)
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/SRC/Bounds.cpp:217
SparseSolver::getZeroPivots
virtual returnValue getZeroPivots(int_t *&zeroPivots)
Definition: SparseSolver.cpp:123
SymmetricMatrix
Abstract base class for interfacing matrix-vector operations tailored to symmetric matrices...
Definition: qpOASES-3.0beta/include/qpOASES/Matrices.hpp:246
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:11