1 /*
2 * This file is part of qpOASES.
3 *
4 * qpOASES -- An Implementation of the Online Active Set Strategy.
5 * Copyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka,
6 * Christian Kirches et al. All rights reserved.
7 *
8 * qpOASES is free software; you can redistribute it and/or
9 * modify it under the terms of the GNU Lesser General Public
10 * License as published by the Free Software Foundation; either
11 * version 2.1 of the License, or (at your option) any later version.
12 *
13 * qpOASES is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
16 * See the GNU Lesser General Public License for more details.
17 *
18 * You should have received a copy of the GNU Lesser General Public
19 * License along with qpOASES; if not, write to the Free Software
20 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
21 *
22 */
23
24
25 /**
26 * \file src/Utils.cpp
27 * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to Eckhard Arnold)
28 * \version 3.2
29 * \date 2007-2017
30 *
31 * Implementation of some utility functions for working with qpOASES.
32 */
33
34
35 #include <math.h>
36
37 #if defined(__WIN32__) || defined(WIN32)
38 #include <windows.h>
39 #elif defined(LINUX) || defined(__LINUX__)
40 #include <sys/stat.h>
41 #include <sys/time.h>
42 #endif
43
44 #ifdef __MATLAB__
45 #include "mex.h"
46 #endif
47
48 #ifdef __SCILAB__
49 #include <scilab/sciprint.h>
50 #endif
51
52
53 #include <qpOASES/Utils.hpp>
54
55
56 #ifdef __NO_SNPRINTF__
57 #if (!defined(_MSC_VER)) || defined(__DSPACE__) || defined(__XPCTARGET__)
58 /* If snprintf is not available, provide an empty implementation. */
snprintf(char * s,size_t n,const char * format,...)59 int snprintf( char* s, size_t n, const char* format, ... )
60 {
61 if ( n > 0 )
62 s[0] = '\0';
63
64 return 0;
65 }
66 #endif
67 #endif /* __NO_SNPRINTF__ */
68
69
70 BEGIN_NAMESPACE_QPOASES
71
72
73 /*
74 * p r i n t
75 */
print(const real_t * const v,int_t n,const char * name)76 returnValue print( const real_t* const v, int_t n, const char* name )
77 {
78 #ifndef __SUPPRESSANYOUTPUT__
79
80 int_t i;
81 char myPrintfString[MAX_STRING_LENGTH];
82
83 /* Print vector name. */
84 if ( name != 0 )
85 {
86 snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
87 myPrintf( myPrintfString );
88 }
89
90 /* Print vector data. */
91 for( i=0; i<n; ++i )
92 {
93 snprintf( myPrintfString,MAX_STRING_LENGTH," %.16e\t", v[i] );
94 myPrintf( myPrintfString );
95 }
96 myPrintf( "\n" );
97
98 #endif /* __SUPPRESSANYOUTPUT__ */
99
100 return SUCCESSFUL_RETURN;
101 }
102
103
104 /*
105 * p r i n t
106 */
print(const real_t * const v,int_t n,const int_t * const V_idx,const char * name)107 returnValue print( const real_t* const v, int_t n, const int_t* const V_idx, const char* name )
108 {
109 #ifndef __SUPPRESSANYOUTPUT__
110
111 int_t i;
112 char myPrintfString[MAX_STRING_LENGTH];
113
114 /* Print vector name. */
115 if ( name != 0 )
116 {
117 snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
118 myPrintf( myPrintfString );
119 }
120
121 /* Print a permuted vector data. */
122 for( i=0; i<n; ++i )
123 {
124 snprintf( myPrintfString,MAX_STRING_LENGTH," %.16e\t", v[ V_idx[i] ] );
125 myPrintf( myPrintfString );
126 }
127 myPrintf( "\n" );
128
129 #endif /* __SUPPRESSANYOUTPUT__ */
130
131 return SUCCESSFUL_RETURN;
132 }
133
134
135 /*
136 * p r i n t
137 */
print(const real_t * const M,int_t nrow,int_t ncol,const char * name)138 returnValue print( const real_t* const M, int_t nrow, int_t ncol, const char* name )
139 {
140 #ifndef __SUPPRESSANYOUTPUT__
141
142 int_t i;
143 char myPrintfString[MAX_STRING_LENGTH];
144
145 /* Print matrix name. */
146 if ( name != 0 )
147 {
148 snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
149 myPrintf( myPrintfString );
150 }
151
152 /* Print a matrix data as a collection of row vectors. */
153 for( i=0; i<nrow; ++i )
154 print( &(M[i*ncol]), ncol );
155 myPrintf( "\n" );
156
157 #endif /* __SUPPRESSANYOUTPUT__ */
158
159 return SUCCESSFUL_RETURN;
160 }
161
162
163 /*
164 * p r i n t
165 */
print(const real_t * const M,int_t nrow,int_t ncol,const int_t * const ROW_idx,const int_t * const COL_idx,const char * name)166 returnValue print( const real_t* const M, int_t nrow, int_t ncol, const int_t* const ROW_idx, const int_t* const COL_idx, const char* name )
167 {
168 #ifndef __SUPPRESSANYOUTPUT__
169
170 int_t i;
171 char myPrintfString[MAX_STRING_LENGTH];
172
173 /* Print matrix name. */
174 if ( name != 0 )
175 {
176 snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
177 myPrintf( myPrintfString );
178 }
179
180 /* Print a permuted matrix data as a collection of permuted row vectors. */
181 for( i=0; i<nrow; ++i )
182 print( &( M[ ROW_idx[i]*ncol ] ), ncol, COL_idx );
183 myPrintf( "\n" );
184
185 #endif /* __SUPPRESSANYOUTPUT__ */
186
187 return SUCCESSFUL_RETURN;
188 }
189
190
191 /*
192 * p r i n t
193 */
print(const int_t * const index,int_t n,const char * name)194 returnValue print( const int_t* const index, int_t n, const char* name )
195 {
196 #ifndef __SUPPRESSANYOUTPUT__
197
198 int_t i;
199 char myPrintfString[MAX_STRING_LENGTH];
200
201 /* Print indexlist name. */
202 if ( name != 0 )
203 {
204 snprintf( myPrintfString,MAX_STRING_LENGTH,"%s = \n", name );
205 myPrintf( myPrintfString );
206 }
207
208 /* Print a indexlist data. */
209 for( i=0; i<n; ++i )
210 {
211 snprintf( myPrintfString,MAX_STRING_LENGTH," %d\t", (int)(index[i]) );
212 myPrintf( myPrintfString );
213 }
214 myPrintf( "\n" );
215
216 #endif /* __SUPPRESSANYOUTPUT__ */
217
218 return SUCCESSFUL_RETURN;
219 }
220
221
222 /*
223 * m y P r i n t f
224 */
myPrintf(const char * s)225 returnValue myPrintf( const char* s )
226 {
227 #ifndef __SUPPRESSANYOUTPUT__
228
229
230 if ( s == 0 )
231 return RET_INVALID_ARGUMENTS;
232
233 #ifdef __MATLAB__
234 mexPrintf( s );
235 #else
236 #ifdef __SCILAB__
237 sciprint( s );
238 #else
239 FILE* outputfile = getGlobalMessageHandler( )->getOutputFile( );
240 if ( outputfile == 0 )
241 return THROWERROR( RET_NO_GLOBAL_MESSAGE_OUTPUTFILE );
242 fprintf( outputfile, "%s", s );
243 #endif /* __SCILAB__ */
244 #endif /* __MATLAB__ */
245
246 #endif /* __SUPPRESSANYOUTPUT__ */
247
248 return SUCCESSFUL_RETURN;
249 }
250
251
252 /*
253 * p r i n t C o p y r i g h t N o t i c e
254 */
printCopyrightNotice()255 returnValue printCopyrightNotice( )
256 {
257 #ifndef __SUPPRESSANYOUTPUT__
258 #ifndef __XPCTARGET__
259 #ifndef __DSPACE__
260 #ifndef __NO_COPYRIGHT__
261 myPrintf( "\nqpOASES -- An Implementation of the Online Active Set Strategy.\nCopyright (C) 2007-2017 by Hans Joachim Ferreau, Andreas Potschka,\nChristian Kirches et al. All rights reserved.\n\nqpOASES is distributed under the terms of the \nGNU Lesser General Public License 2.1 in the hope that it will be \nuseful, but WITHOUT ANY WARRANTY; without even the implied warranty \nof MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. \nSee the GNU Lesser General Public License for more details.\n\n" );
262 #endif /* __NO_COPYRIGHT__ */
263 #endif /* __DSPACE__ */
264 #endif /* __XPCTARGET__ */
265 #endif /* __SUPPRESSANYOUTPUT__ */
266 return SUCCESSFUL_RETURN;
267 }
268
269
270 /*
271 * r e a d F r o m F i l e
272 */
readFromFile(real_t * data,int_t nrow,int_t ncol,const char * datafilename)273 returnValue readFromFile( real_t* data, int_t nrow, int_t ncol,
274 const char* datafilename
275 )
276 {
277 #ifndef __SUPPRESSANYOUTPUT__
278
279 int_t i, j;
280 real_t float_data;
281 FILE* datafile;
282
283 /* 1) Open file. */
284 if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
285 {
286 char errstr[MAX_STRING_LENGTH];
287 snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
288 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
289 }
290
291 /* 2) Read data from file. */
292 for( i=0; i<nrow; ++i )
293 {
294 for( j=0; j<ncol; ++j )
295 {
296 #ifdef __USE_SINGLE_PRECISION__
297 if ( fscanf( datafile, "%f ", &float_data ) == 0 )
298 #else
299 if ( fscanf( datafile, "%lf ", &float_data ) == 0 )
300 #endif /* __USE_SINGLE_PRECISION__ */
301 {
302 fclose( datafile );
303 char errstr[MAX_STRING_LENGTH];
304 snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
305 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
306 }
307 data[i*ncol + j] = ( (real_t) float_data );
308 }
309 }
310
311 /* 3) Close file. */
312 fclose( datafile );
313
314 return SUCCESSFUL_RETURN;
315
316 #else /* __SUPPRESSANYOUTPUT__ */
317
318 return RET_NOT_YET_IMPLEMENTED;
319
320 #endif /* __SUPPRESSANYOUTPUT__ */
321 }
322
323
324 /*
325 * r e a d F r o m F i l e
326 */
readFromFile(real_t * data,int_t n,const char * datafilename)327 returnValue readFromFile( real_t* data, int_t n,
328 const char* datafilename
329 )
330 {
331 return readFromFile( data, n, 1, datafilename );
332 }
333
334
335
336 /*
337 * r e a d F r o m F i l e
338 */
readFromFile(int_t * data,int_t n,const char * datafilename)339 returnValue readFromFile( int_t* data, int_t n,
340 const char* datafilename
341 )
342 {
343 #ifndef __SUPPRESSANYOUTPUT__
344
345 int_t i;
346 FILE* datafile;
347
348 /* 1) Open file. */
349 if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
350 {
351 char errstr[MAX_STRING_LENGTH];
352 snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
353 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
354 }
355
356 /* 2) Read data from file. */
357 for( i=0; i<n; ++i )
358 {
359 #ifdef __USE_LONG_INTEGERS__
360 if ( fscanf( datafile, "%ld\n", &(data[i]) ) == 0 )
361 #else
362 if ( fscanf( datafile, "%d\n", &(data[i]) ) == 0 )
363 #endif
364 {
365 fclose( datafile );
366 char errstr[MAX_STRING_LENGTH];
367 snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
368 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
369 }
370 }
371
372 /* 3) Close file. */
373 fclose( datafile );
374
375 return SUCCESSFUL_RETURN;
376
377 #else /* __SUPPRESSANYOUTPUT__ */
378
379 return RET_NOT_YET_IMPLEMENTED;
380
381 #endif /* __SUPPRESSANYOUTPUT__ */
382 }
383
384
385 /*
386 * w r i t e I n t o F i l e
387 */
writeIntoFile(const real_t * const data,int_t nrow,int_t ncol,const char * datafilename,BooleanType append)388 returnValue writeIntoFile( const real_t* const data, int_t nrow, int_t ncol,
389 const char* datafilename, BooleanType append
390 )
391 {
392 #ifndef __SUPPRESSANYOUTPUT__
393
394 int_t i, j;
395 FILE* datafile;
396
397 /* 1) Open file. */
398 if ( append == BT_TRUE )
399 {
400 /* append data */
401 if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
402 {
403 char errstr[MAX_STRING_LENGTH];
404 snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
405 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
406 }
407 }
408 else
409 {
410 /* do not append data */
411 if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
412 {
413 char errstr[MAX_STRING_LENGTH];
414 snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
415 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
416 }
417 }
418
419 /* 2) Write data into file. */
420 for( i=0; i<nrow; ++i )
421 {
422 for( j=0; j<ncol; ++j )
423 fprintf( datafile, "%.16e ", data[i*ncol+j] );
424
425 fprintf( datafile, "\n" );
426 }
427
428 /* 3) Close file. */
429 fclose( datafile );
430
431 return SUCCESSFUL_RETURN;
432
433 #else /* __SUPPRESSANYOUTPUT__ */
434
435 return RET_NOT_YET_IMPLEMENTED;
436
437 #endif /* __SUPPRESSANYOUTPUT__ */
438 }
439
440
441 /*
442 * w r i t e I n t o F i l e
443 */
writeIntoFile(const real_t * const data,int_t n,const char * datafilename,BooleanType append)444 returnValue writeIntoFile( const real_t* const data, int_t n,
445 const char* datafilename, BooleanType append
446 )
447 {
448 return writeIntoFile( data,1,n,datafilename,append );
449 }
450
451
452 /*
453 * w r i t e I n t o F i l e
454 */
writeIntoFile(const int_t * const integer,int_t n,const char * datafilename,BooleanType append)455 returnValue writeIntoFile( const int_t* const integer, int_t n,
456 const char* datafilename, BooleanType append
457 )
458 {
459 #ifndef __SUPPRESSANYOUTPUT__
460
461 int_t i;
462
463 FILE* datafile;
464
465 /* 1) Open file. */
466 if ( append == BT_TRUE )
467 {
468 /* append data */
469 if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
470 {
471 char errstr[MAX_STRING_LENGTH];
472 snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
473 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
474 }
475 }
476 else
477 {
478 /* do not append data */
479 if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
480 {
481 char errstr[MAX_STRING_LENGTH];
482 snprintf( errstr,MAX_STRING_LENGTH,"(%s)",datafilename );
483 return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNC__,__FILE__,__LINE__,VS_VISIBLE );
484 }
485 }
486
487 /* 2) Write data into file. */
488 for( i=0; i<n; ++i )
489 fprintf( datafile, "%d\n",(int)(integer[i]) );
490
491 /* 3) Close file. */
492 fclose( datafile );
493
494 return SUCCESSFUL_RETURN;
495
496 #else /* __SUPPRESSANYOUTPUT__ */
497
498 return RET_NOT_YET_IMPLEMENTED;
499
500 #endif /* __SUPPRESSANYOUTPUT__ */
501 }
502
503
504 /*
505 * w r i t e I n t o M a t F i l e
506 */
writeIntoMatFile(FILE * const matFile,const real_t * const data,int_t nRows,int_t nCols,const char * name)507 returnValue writeIntoMatFile( FILE* const matFile,
508 const real_t* const data, int_t nRows, int_t nCols, const char* name
509 )
510 {
511 /* Note, this code snippet has been inspired from the document
512 * "Matlab(R) MAT-file Format, R2013b" by MathWorks */
513
514 #ifndef __SUPPRESSANYOUTPUT__
515
516 if ( ( matFile == 0 ) || ( data == 0 ) || ( nRows < 0 ) || ( nCols < 0 ) || ( name == 0 ) )
517 return RET_INVALID_ARGUMENTS;
518
519 MatMatrixHeader var;
520
521 // setup variable header
522 var.numericFormat = 0000; /* IEEE Little Endian - reserved - double precision (64 bits) - numeric full matrix */
523 var.nRows = nRows; /* number of rows */
524 var.nCols = nCols; /* number of columns */
525 var.imaginaryPart = 0; /* no imaginary part */
526 var.nCharName = (long)(strlen(name))+1; /* matrix name length */
527
528 /* write variable header to mat file */
529 if ( fwrite( &var, sizeof(MatMatrixHeader),1, matFile ) < 1 )
530 return RET_UNABLE_TO_WRITE_FILE;
531
532 if ( fwrite( name, sizeof(char),(unsigned long)(var.nCharName), matFile ) < 1 )
533 return RET_UNABLE_TO_WRITE_FILE;
534
535 int_t ii, jj;
536 double curData;
537
538 for ( ii=0; ii<nCols; ++ii )
539 for ( jj=0; jj<nRows; ++jj )
540 {
541 curData = (real_t)data[jj*nCols+ii];
542 if ( fwrite( &curData, sizeof(double),1, matFile ) < 1 )
543 return RET_UNABLE_TO_WRITE_FILE;
544 }
545
546 return SUCCESSFUL_RETURN;
547
548 #else /* __SUPPRESSANYOUTPUT__ */
549
550 return RET_NOT_YET_IMPLEMENTED;
551
552 #endif /* __SUPPRESSANYOUTPUT__ */
553 }
554
555
556 /*
557 * w r i t e I n t o M a t F i l e
558 */
writeIntoMatFile(FILE * const matFile,const int_t * const data,int_t nRows,int_t nCols,const char * name)559 returnValue writeIntoMatFile( FILE* const matFile,
560 const int_t* const data, int_t nRows, int_t nCols, const char* name
561 )
562 {
563 real_t* realData = new real_t[nRows*nCols];
564
565 int_t ii, jj;
566
567 for ( ii=0; ii<nRows; ++ii )
568 for ( jj=0; jj<nCols; ++jj )
569 realData[ ii*nCols+jj ] = (real_t) data[ ii*nCols+jj ];
570
571 returnValue returnvalue = writeIntoMatFile( matFile,realData,nRows,nCols,name );
572 delete[] realData;
573
574 return returnvalue;
575 }
576
577
578 /*
579 * g e t C P U t i m e
580 */
getCPUtime()581 real_t getCPUtime( )
582 {
583 real_t current_time = -1.0;
584
585 #if defined(__WIN32__) || defined(WIN32)
586 LARGE_INTEGER counter, frequency;
587 QueryPerformanceFrequency(&frequency);
588 QueryPerformanceCounter(&counter);
589 current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);
590 #elif defined(LINUX) || defined(__LINUX__)
591 struct timeval theclock;
592 gettimeofday( &theclock,0 );
593 current_time = 1.0*(real_t) theclock.tv_sec + 1.0e-6* (real_t) theclock.tv_usec;
594 #endif
595
596 return current_time;
597 }
598
599
600 /*
601 * g e t N o r m
602 */
getNorm(const real_t * const v,int_t n,int_t type)603 real_t getNorm( const real_t* const v, int_t n, int_t type )
604 {
605 int_t i;
606
607 real_t norm = 0.0;
608
609 switch ( type )
610 {
611 case 2:
612 for( i=0; i<n; ++i )
613 norm += v[i]*v[i];
614 return getSqrt( norm );
615
616 case 1:
617 for( i=0; i<n; ++i )
618 norm += getAbs( v[i] );
619 return norm;
620
621 default:
622 THROWERROR( RET_INVALID_ARGUMENTS );
623 return -INFTY;
624 }
625 }
626
627
628 /*
629 * g e t K k t V i o l a t i o n
630 */
getKktViolation(int_t nV,int_t nC,const real_t * const H,const real_t * const g,const real_t * const A,const real_t * const lb,const real_t * const ub,const real_t * const lbA,const real_t * const ubA,const real_t * const x,const real_t * const y,real_t & stat,real_t & feas,real_t & cmpl,const real_t * const workingSetB,const real_t * const workingSetC,BooleanType hasIdentityHessian)631 returnValue getKktViolation( int_t nV, int_t nC,
632 const real_t* const H, const real_t* const g, const real_t* const A,
633 const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
634 const real_t* const x, const real_t* const y,
635 real_t& stat, real_t& feas, real_t& cmpl,
636 const real_t* const workingSetB, const real_t* const workingSetC, BooleanType hasIdentityHessian
637 )
638 {
639 /* Tolerance for dual variables considered zero. */
640 const real_t dualActiveTolerance = 1.0e3 * EPS;
641
642 int_t i, j;
643 real_t sum, prod;
644
645 /* Initialize residuals */
646 stat = feas = cmpl = 0.0;
647
648 /* check stationarity */
649 for (i = 0; i < nV; i++)
650 {
651 /* g term and variable bounds dual term */
652 if ( g != 0 )
653 sum = g[i] - y[i];
654 else
655 sum = 0.0 - y[i];
656
657 /* H*x term */
658 if ( H != 0 )
659 for (j = 0; j < nV; j++) sum += H[i*nV+j] * x[j];
660 else
661 {
662 if ( hasIdentityHessian == BT_TRUE )
663 sum += x[i];
664 }
665
666 /* A'*y term */
667 if ( A != 0 )
668 for (j = 0; j < nC; j++) sum -= A[j*nV+i] * y[nV+j];
669
670 /* update stat */
671 if (getAbs(sum) > stat) stat = getAbs(sum);
672 }
673
674 /* check primal feasibility and complementarity of bounds */
675 /* feasibility */
676 for (i = 0; i < nV; i++)
677 {
678 if ( lb != 0 )
679 if (lb[i] - x[i] > feas)
680 feas = lb[i] - x[i];
681
682 if ( ub != 0 )
683 if (x[i] - ub[i] > feas)
684 feas = x[i] - ub[i];
685 }
686
687 /* complementarity */
688 if ( workingSetB == 0 )
689 {
690 for (i = 0; i < nV; i++)
691 {
692 prod = 0.0;
693
694 /* lower bound */
695 if ( lb != 0 )
696 if (y[i] > dualActiveTolerance)
697 prod = (x[i] - lb[i]) * y[i];
698
699 /* upper bound */
700 if ( ub != 0 )
701 if (y[i] < -dualActiveTolerance)
702 prod = (x[i] - ub[i]) * y[i];
703
704 if (getAbs(prod) > cmpl) cmpl = getAbs(prod);
705 }
706 }
707 else
708 {
709 for (i = 0; i < nV; i++)
710 {
711 prod = 0.0;
712
713 /* lower bound */
714 if ( lb != 0 )
715 {
716 if ( isEqual(workingSetB[i],-1.0) == BT_TRUE )
717 prod = (x[i] - lb[i]) * y[i];
718 }
719
720 /* upper bound */
721 if ( ub != 0 )
722 {
723 if ( isEqual(workingSetB[i],1.0) == BT_TRUE )
724 prod = (x[i] - ub[i]) * y[i];
725 }
726
727 if (getAbs(prod) > cmpl) cmpl = getAbs(prod);
728 }
729 }
730
731 /* check primal feasibility and complementarity of constraints */
732 for (i = 0; i < nC; i++)
733 {
734 /* compute sum = (A*x)_i */
735 sum = 0.0;
736 if ( A != 0 )
737 for (j = 0; j < nV; j++)
738 sum += A[i*nV+j] * x[j];
739
740 /* feasibility */
741 if ( lbA != 0 )
742 if (lbA[i] - sum > feas)
743 feas = lbA[i] - sum;
744
745 if ( ubA != 0 )
746 if (sum - ubA[i] > feas)
747 feas = sum - ubA[i];
748
749 /* complementarity */
750 prod = 0.0;
751
752 /* lower bound */
753 if ( lbA != 0 )
754 {
755 if ( workingSetC == 0 )
756 {
757 if (y[nV+i] > dualActiveTolerance)
758 prod = (sum - lbA[i]) * y[nV+i];
759 }
760 else
761 {
762 if ( isEqual(workingSetC[i],-1.0) == BT_TRUE )
763 prod = (sum - lbA[i]) * y[nV+i];
764 }
765 }
766
767 /* upper bound */
768 if ( ubA != 0 )
769 {
770 if ( workingSetC == 0 )
771 {
772 if (y[nV+i] < -dualActiveTolerance)
773 prod = (sum - ubA[i]) * y[nV+i];
774 }
775 else
776 {
777 if ( isEqual(workingSetC[i],1.0) == BT_TRUE )
778 prod = (sum - ubA[i]) * y[nV+i];
779 }
780 }
781
782 if (getAbs(prod) > cmpl) cmpl = getAbs(prod);
783 }
784
785 return SUCCESSFUL_RETURN;
786 }
787
788
789 /*
790 * g e t K k t V i o l a t i o n
791 */
getKktViolation(int_t nV,const real_t * const H,const real_t * const g,const real_t * const lb,const real_t * const ub,const real_t * const x,const real_t * const y,real_t & stat,real_t & feas,real_t & cmpl,const real_t * const workingSetB,BooleanType hasIdentityHessian)792 returnValue getKktViolation( int_t nV,
793 const real_t* const H, const real_t* const g,
794 const real_t* const lb, const real_t* const ub,
795 const real_t* const x, const real_t* const y,
796 real_t& stat, real_t& feas, real_t& cmpl,
797 const real_t* const workingSetB, BooleanType hasIdentityHessian
798 )
799 {
800 return getKktViolation( nV,0,
801 H,g,0,lb,ub,0,0,
802 x,y,
803 stat,feas,cmpl,
804 workingSetB,0,hasIdentityHessian
805 );
806 }
807
808
809 /*
810 * c o n v e r t B o o l e a n T y p e T o S t r i n g
811 */
convertBooleanTypeToString(BooleanType value,char * const string)812 returnValue convertBooleanTypeToString( BooleanType value, char* const string )
813 {
814 #ifndef __SUPPRESSANYOUTPUT__
815 if ( value == BT_FALSE )
816 snprintf( string,20,"BT_FALSE" );
817 else
818 snprintf( string,20,"BT_TRUE" );
819 #endif /* __SUPPRESSANYOUTPUT__ */
820
821 return SUCCESSFUL_RETURN;
822 }
823
824
825 /*
826 * c o n v e r t S u b j e c t T o S t a t u s T o S t r i n g
827 */
convertSubjectToStatusToString(SubjectToStatus value,char * const string)828 returnValue convertSubjectToStatusToString( SubjectToStatus value, char* const string )
829 {
830 #ifndef __SUPPRESSANYOUTPUT__
831 switch( value )
832 {
833 case ST_INACTIVE:
834 snprintf( string,20,"ST_INACTIVE" );
835 break;
836
837 case ST_LOWER:
838 snprintf( string,20,"ST_LOWER" );
839 break;
840
841 case ST_UPPER:
842 snprintf( string,20,"ST_UPPER" );
843 break;
844
845 case ST_UNDEFINED:
846 snprintf( string,20,"ST_UNDEFINED" );
847 break;
848
849 case ST_INFEASIBLE_LOWER:
850 snprintf( string,20,"ST_INFEASIBLE_LOWER" );
851 break;
852
853 case ST_INFEASIBLE_UPPER:
854 snprintf( string,20,"ST_INFEASIBLE_UPPER" );
855 break;
856
857 default:
858 snprintf( string,20,"<invalid value>" );
859 break;
860 }
861 #endif /* __SUPPRESSANYOUTPUT__ */
862
863 return SUCCESSFUL_RETURN;
864 }
865
866
867 /*
868 * c o n v e r t P r i n t L e v e l T o S t r i n g
869 */
convertPrintLevelToString(PrintLevel value,char * const string)870 returnValue convertPrintLevelToString( PrintLevel value, char* const string )
871 {
872 #ifndef __SUPPRESSANYOUTPUT__
873 switch( value )
874 {
875 case PL_NONE:
876 snprintf( string,20,"PL_NONE" );
877 break;
878
879 case PL_LOW:
880 snprintf( string,20,"PL_LOW" );
881 break;
882
883 case PL_MEDIUM:
884 snprintf( string,20,"PL_MEDIUM" );
885 break;
886
887 case PL_HIGH:
888 snprintf( string,20,"PL_HIGH" );
889 break;
890
891 case PL_TABULAR:
892 snprintf( string,20,"PL_TABULAR" );
893 break;
894
895 case PL_DEBUG_ITER:
896 snprintf( string,20,"PL_DEBUG_ITER" );
897 break;
898
899 default:
900 snprintf( string,20,"<invalid value>" );
901 break;
902 }
903 #endif /* __SUPPRESSANYOUTPUT__ */
904
905 return SUCCESSFUL_RETURN;
906 }
907
908
909 /*
910 * g e t S i m p l e S t a t u s
911 */
getSimpleStatus(returnValue returnvalue,BooleanType doPrintStatus)912 int_t getSimpleStatus( returnValue returnvalue,
913 BooleanType doPrintStatus
914 )
915 {
916 int_t simpleStatus = -1;
917
918 /* determine simple status from returnvalue */
919 switch ( returnvalue )
920 {
921 case SUCCESSFUL_RETURN:
922 simpleStatus = 0;
923 break;
924
925 case RET_MAX_NWSR_REACHED:
926 simpleStatus = 1;
927 break;
928
929 case RET_INIT_FAILED_INFEASIBILITY:
930 case RET_HOTSTART_STOPPED_INFEASIBILITY:
931 simpleStatus = -2;
932 break;
933
934 case RET_INIT_FAILED_UNBOUNDEDNESS:
935 case RET_HOTSTART_STOPPED_UNBOUNDEDNESS:
936 simpleStatus = -3;
937 break;
938
939 default:
940 simpleStatus = -1;
941 break;
942 }
943
944 if ( doPrintStatus == BT_TRUE )
945 {
946 VisibilityStatus vsInfo = getGlobalMessageHandler( )->getInfoVisibilityStatus( );
947 getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_VISIBLE );
948 getGlobalMessageHandler( )->setErrorCount( -1 );
949
950 int_t retValNumber = (int_t)RET_SIMPLE_STATUS_P0 - simpleStatus;
951 THROWINFO( (returnValue)retValNumber );
952
953 getGlobalMessageHandler( )->setInfoVisibilityStatus( vsInfo );
954 }
955
956 return simpleStatus;
957 }
958
959
960 /*
961 * n o r m a l i s e C o n s t r a i n t s
962 */
normaliseConstraints(int_t nV,int_t nC,real_t * A,real_t * lbA,real_t * ubA,int_t type)963 returnValue normaliseConstraints( int_t nV, int_t nC,
964 real_t* A, real_t* lbA, real_t* ubA,
965 int_t type
966 )
967 {
968 int_t ii, jj;
969 real_t curNorm;
970
971 if ( ( nV <= 0 ) || ( nC <= 0 ) || ( A == 0 ) )
972 return THROWERROR( RET_INVALID_ARGUMENTS );
973
974 for( ii=0; ii<nC; ++ii )
975 {
976 /* get row norm */
977 curNorm = getNorm( &(A[ii*nV]),nV,type );
978
979 if ( curNorm > EPS )
980 {
981 /* normalise if norm is positive */
982 for( jj=0; jj<nV; ++jj )
983 A[ii*nV + jj] /= curNorm;
984
985 if ( lbA != 0 ) lbA[ii] /= curNorm;
986 if ( ubA != 0 ) ubA[ii] /= curNorm;
987 }
988 else
989 {
990 /* if row norm is (close to) zero, kind of erase constraint */
991 if ( type == 1 )
992 {
993 for( jj=0; jj<nV; ++jj )
994 A[ii*nV + jj] = 1.0 / ((real_t)nV);
995 }
996 else
997 {
998 /* assume type == 2 */
999 for( jj=0; jj<nV; ++jj )
1000 A[ii*nV + jj] = 1.0 / getSqrt((real_t)nV);
1001 }
1002
1003 if ( lbA != 0 ) lbA[ii] = -INFTY;
1004 if ( ubA != 0 ) ubA[ii] = INFTY;
1005 }
1006 }
1007
1008 return SUCCESSFUL_RETURN;
1009 }
1010
1011
1012 #ifdef __DEBUG__
1013 /*
1014 * g d b _ p r i n t m at
1015 */
gdb_printmat(const char * fname,real_t * M,int_t n,int_t m,int_t ldim)1016 extern "C" void gdb_printmat(const char *fname, real_t *M, int_t n, int_t m, int_t ldim)
1017 {
1018 #ifndef __SUPPRESSANYOUTPUT__
1019
1020 int_t i, j;
1021 FILE *fid;
1022
1023 fid = fopen(fname, "wt");
1024 if (!fid)
1025 {
1026 perror("Error opening file: ");
1027 return;
1028 }
1029
1030 for (i = 0; i < n; i++)
1031 {
1032 for (j = 0; j < m; j++)
1033 fprintf(fid, " %23.16e", M[j*ldim+i]);
1034 fprintf(fid, "\n");
1035 }
1036 fclose(fid);
1037
1038 #endif /* __SUPPRESSANYOUTPUT__ */
1039 }
1040 #endif /* __DEBUG__ */
1041
1042
1043
1044 #if defined(__DSPACE__) || defined(__XPCTARGET__) || defined(__C_WRAPPER__)
1045 /*
1046 * _ _ c x a _ p u r e _ v i r t u a l
1047 */
__cxa_pure_virtual(void)1048 void __cxa_pure_virtual( void )
1049 {
1050 /* put your customized implementation here! */
1051 }
1052 #endif /* __DSPACE__ || __XPCTARGET__ || __C_WRAPPER__ */
1053
1054
1055
1056 END_NAMESPACE_QPOASES
1057
1058
1059 /*
1060 * end of file
1061 */
1062