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