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 interfaces/matlab/qpOASES_matlab_utils.cpp
27 * \author Hans Joachim Ferreau, Alexander Buchner
28 * \version 3.2
29 * \date 2007-2017
30 *
31 * Collects utility functions for Interface to Matlab(R) that
32 * enables to call qpOASES as a MEX function.
33 *
34 */
35
36
37
QPInstance(uint_t _nV,uint_t _nC,HessianType _hessianType,BooleanType _isSimplyBounded,BooleanType _sparseLA)38 QPInstance::QPInstance( uint_t _nV, uint_t _nC, HessianType _hessianType,
39 BooleanType _isSimplyBounded, BooleanType _sparseLA
40 )
41 : sparseLA(_sparseLA)
42 {
43 handle = s_nexthandle++;
44
45 if ( _nC > 0 )
46 isSimplyBounded = BT_FALSE;
47 else
48 isSimplyBounded = _isSimplyBounded;
49
50 if ( isSimplyBounded == BT_TRUE && sparseLA == BT_FALSE )
51 {
52 sqp = 0;
53 qpb = new QProblemB( _nV,_hessianType );
54 }
55 else if ( sparseLA == BT_FALSE )
56 {
57 sqp = new SQProblem( _nV,_nC,_hessianType );
58 qpb = 0;
59 }
60 else
61 {
62 #ifdef SOLVER_MA57
63 sqp = new SQProblemSchur( _nV,_nC,_hessianType );
64 #else
65 sqp = new SQProblem( _nV,_nC,_hessianType );
66 #endif
67 qpb = 0;
68 }
69
70 H = 0;
71 A = 0;
72 Hir = 0;
73 Hjc = 0;
74 Air = 0;
75 Ajc = 0;
76 Hv = 0;
77 Av = 0;
78 }
79
80
~QPInstance()81 QPInstance::~QPInstance( )
82 {
83 deleteQPMatrices();
84
85 if ( sqp != 0 )
86 {
87 delete sqp;
88 sqp = 0;
89 }
90
91 if ( qpb != 0 )
92 {
93 delete qpb;
94 qpb = 0;
95 }
96 }
97
98
deleteQPMatrices()99 returnValue QPInstance::deleteQPMatrices( )
100 {
101 if ( H != 0 )
102 {
103 delete H;
104 H = 0;
105 }
106
107 if ( Hv != 0 )
108 {
109 delete[] Hv;
110 Hv = 0;
111 }
112
113 if ( Hjc != 0 )
114 {
115 delete[] Hjc;
116 Hjc = 0;
117 }
118
119 if ( Hir != 0 )
120 {
121 delete[] Hir;
122 Hir = 0;
123 }
124
125 if ( A != 0 )
126 {
127 delete A;
128 A = 0;
129 }
130
131 if ( Av != 0 )
132 {
133 delete[] Av;
134 Av = 0;
135 }
136
137 if ( Ajc != 0 )
138 {
139 delete[] Ajc;
140 Ajc = 0;
141 }
142
143 if ( Air != 0 )
144 {
145 delete[] Air;
146 Air = 0;
147 }
148
149 return SUCCESSFUL_RETURN;
150 }
151
152
getNV() const153 int_t QPInstance::getNV() const
154 {
155 if ( sqp != 0 )
156 return sqp->getNV();
157
158 if ( qpb != 0 )
159 return qpb->getNV();
160
161 return 0;
162 }
163
164
getNC() const165 int_t QPInstance::getNC() const
166 {
167 if ( sqp != 0 )
168 return sqp->getNC();
169
170 return 0;
171 }
172
173
174
175 /*
176 * m x I s S c a l a r
177 */
mxIsScalar(const mxArray * pm)178 bool mxIsScalar( const mxArray *pm )
179 {
180 if ( ( mxGetM(pm) == 1 ) && ( mxGetN(pm) == 1 ) )
181 return true;
182 else
183 return false;
184 }
185
186
187
188 /*
189 * a l l o c a t e Q P r o b l e m I n s t a n c e
190 */
allocateQPInstance(int_t nV,int_t nC,HessianType hessianType,BooleanType isSimplyBounded,BooleanType isSparse,const Options * options)191 int_t allocateQPInstance( int_t nV, int_t nC, HessianType hessianType,
192 BooleanType isSimplyBounded, BooleanType isSparse, const Options* options
193 )
194 {
195 QPInstance* inst = new QPInstance( nV,nC,hessianType, isSimplyBounded, isSparse );
196
197 if ( ( inst->sqp != 0 ) && ( options != 0 ) )
198 inst->sqp->setOptions( *options );
199
200 if ( ( inst->qpb != 0 ) && ( options != 0 ) )
201 inst->qpb->setOptions( *options );
202
203 g_instances.push_back(inst);
204 return inst->handle;
205 }
206
207
208 /*
209 * g e t Q P r o b l e m I n s t a n c e
210 */
getQPInstance(int_t handle)211 QPInstance* getQPInstance( int_t handle )
212 {
213 uint_t ii;
214 // TODO: this may become slow ...
215 for (ii = 0; ii < g_instances.size (); ++ii)
216 if (g_instances[ii]->handle == handle)
217 return g_instances[ii];
218 return 0;
219 }
220
221
222 /*
223 * d e l e t e Q P r o b l e m I n s t a n c e
224 */
deleteQPInstance(int_t handle)225 void deleteQPInstance( int_t handle )
226 {
227 QPInstance *instance = getQPInstance (handle);
228 if (instance != 0) {
229 for (std::vector<QPInstance*>::iterator itor = g_instances.begin ();
230 itor != g_instances.end (); ++itor)
231 if ((*itor)->handle == handle) {
232 g_instances.erase (itor);
233 break;
234 }
235 delete instance;
236 }
237 }
238
239
240
241 /*
242 * s m a r t D i m e n s i o n C h e c k
243 */
smartDimensionCheck(real_t ** input,uint_t m,uint_t n,BooleanType emptyAllowed,const mxArray * prhs[],int_t idx)244 returnValue smartDimensionCheck( real_t** input, uint_t m, uint_t n, BooleanType emptyAllowed,
245 const mxArray* prhs[], int_t idx
246 )
247 {
248 /* If index is negative, the input does not exist. */
249 if ( idx < 0 )
250 {
251 *input = 0;
252 return SUCCESSFUL_RETURN;
253 }
254
255 /* Otherwise the input has been passed by the user. */
256 if ( mxIsEmpty( prhs[ idx ] ) )
257 {
258 /* input is empty */
259 if ( ( emptyAllowed == BT_TRUE ) || ( idx == 0 ) ) /* idx==0 used for auxInput */
260 {
261 *input = 0;
262 return SUCCESSFUL_RETURN;
263 }
264 else
265 {
266 char msg[MAX_STRING_LENGTH];
267 if ( idx > 0 )
268 snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Empty argument %d not allowed!", idx+1);
269 myMexErrMsgTxt( msg );
270 return RET_INVALID_ARGUMENTS;
271 }
272 }
273 else
274 {
275 /* input is non-empty */
276 if ( mxIsSparse( prhs[ idx ] ) == 0 )
277 {
278 if ( ( mxGetM( prhs[ idx ] ) == m ) && ( mxGetN( prhs[ idx ] ) == n ) )
279 {
280 *input = (real_t*) mxGetPr( prhs[ idx ] );
281 return SUCCESSFUL_RETURN;
282 }
283 else
284 {
285 char msg[MAX_STRING_LENGTH];
286 if ( idx > 0 )
287 snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Input dimension mismatch for argument %d ([%ld,%ld] ~= [%d,%d]).",
288 idx+1, (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), (int)m,(int)n);
289 else /* idx==0 used for auxInput */
290 snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Input dimension mismatch for some auxInput entry ([%ld,%ld] ~= [%d,%d]).",
291 (long int)mxGetM(prhs[idx]), (long int)mxGetN(prhs[idx]), (int)m,(int)n);
292 myMexErrMsgTxt( msg );
293 return RET_INVALID_ARGUMENTS;
294 }
295 }
296 else
297 {
298 char msg[MAX_STRING_LENGTH];
299 if ( idx > 0 )
300 snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Vector argument %d must not be in sparse format!", idx+1);
301 else /* idx==0 used for auxInput */
302 snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): auxInput entries must not be in sparse format!" );
303 myMexErrMsgTxt( msg );
304 return RET_INVALID_ARGUMENTS;
305 }
306 }
307
308 return SUCCESSFUL_RETURN;
309 }
310
311
312
313 /*
314 * c o n t a i n s N a N
315 */
containsNaN(const real_t * const data,uint_t dim)316 BooleanType containsNaN( const real_t* const data, uint_t dim )
317 {
318 uint_t i;
319
320 if ( data == 0 )
321 return BT_FALSE;
322
323 for ( i = 0; i < dim; ++i )
324 if ( mxIsNaN(data[i]) == 1 )
325 return BT_TRUE;
326
327 return BT_FALSE;
328 }
329
330
331 /*
332 * c o n t a i n s I n f
333 */
containsInf(const real_t * const data,uint_t dim)334 BooleanType containsInf( const real_t* const data, uint_t dim )
335 {
336 uint_t i;
337
338 if ( data == 0 )
339 return BT_FALSE;
340
341 for ( i = 0; i < dim; ++i )
342 if ( mxIsInf(data[i]) == 1 )
343 return BT_TRUE;
344
345 return BT_FALSE;
346 }
347
348
349 /*
350 * c o n t a i n s N a N o r I n f
351 */
containsNaNorInf(const mxArray * prhs[],int_t rhs_index,bool mayContainInf)352 BooleanType containsNaNorInf( const mxArray* prhs[], int_t rhs_index,
353 bool mayContainInf
354 )
355 {
356 uint_t dim;
357 char msg[MAX_STRING_LENGTH];
358
359 if ( rhs_index < 0 )
360 return BT_FALSE;
361
362 /* overwrite dim for sparse matrices */
363 if (mxIsSparse(prhs[rhs_index]) == 1)
364 dim = (uint_t)mxGetNzmax(prhs[rhs_index]);
365 else
366 dim = (uint_t)(mxGetM(prhs[rhs_index]) * mxGetN(prhs[rhs_index]));
367
368 if (containsNaN((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
369 snprintf(msg, MAX_STRING_LENGTH,
370 "ERROR (qpOASES): Argument %d contains 'NaN' !", rhs_index + 1);
371 myMexErrMsgTxt(msg);
372 return BT_TRUE;
373 }
374
375 if (mayContainInf == 0) {
376 if (containsInf((real_t*) mxGetPr(prhs[rhs_index]), dim) == BT_TRUE) {
377 snprintf(msg, MAX_STRING_LENGTH,
378 "ERROR (qpOASES): Argument %d contains 'Inf' !",
379 rhs_index + 1);
380 myMexErrMsgTxt(msg);
381 return BT_TRUE;
382 }
383 }
384
385 return BT_FALSE;
386 }
387
388
389 /*
390 * c o n v e r t F o r t r a n T o C
391 */
convertFortranToC(const real_t * const M_for,int_t nV,int_t nC,real_t * const M)392 returnValue convertFortranToC( const real_t* const M_for, int_t nV, int_t nC, real_t* const M )
393 {
394 int_t i,j;
395
396 if ( ( M_for == 0 ) || ( M == 0 ) )
397 return RET_INVALID_ARGUMENTS;
398
399 if ( ( nV < 0 ) || ( nC < 0 ) )
400 return RET_INVALID_ARGUMENTS;
401
402 for ( i=0; i<nC; ++i )
403 for ( j=0; j<nV; ++j )
404 M[i*nV + j] = M_for[j*nC + i];
405
406 return SUCCESSFUL_RETURN;
407 }
408
409
410 /*
411 * h a s O p t i o n s V a l u e
412 */
hasOptionsValue(const mxArray * optionsPtr,const char * const optionString,double ** optionValue)413 BooleanType hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, double** optionValue )
414 {
415 mxArray* optionName = mxGetField( optionsPtr,0,optionString );
416
417 if ( optionName == 0 )
418 {
419 char msg[MAX_STRING_LENGTH];
420 snprintf(msg, MAX_STRING_LENGTH, "Option struct does not contain entry '%s', using default value instead!", optionString );
421 mexWarnMsgTxt( msg );
422 return BT_FALSE;
423 }
424
425 if ( ( mxIsEmpty(optionName) == false ) && ( mxIsScalar( optionName ) == true ) )
426 {
427 *optionValue = mxGetPr( optionName );
428 return BT_TRUE;
429 }
430 else
431 {
432 char msg[MAX_STRING_LENGTH];
433 snprintf(msg, MAX_STRING_LENGTH, "Option '%s' is not a scalar, using default value instead!", optionString );
434 mexWarnMsgTxt( msg );
435 return BT_FALSE;
436 }
437 }
438
439
440 /*
441 * s e t u p O p t i o n s
442 */
setupOptions(Options * options,const mxArray * optionsPtr,int_t & nWSRin,real_t & maxCpuTime)443 returnValue setupOptions( Options* options, const mxArray* optionsPtr, int_t& nWSRin, real_t& maxCpuTime )
444 {
445 double* optionValue;
446 int_t optionValueInt;
447
448 /* Check for correct number of option entries;
449 * may occur, e.g., if user types options.<misspelledName> = <someValue>; */
450 if ( mxGetNumberOfFields(optionsPtr) != 31 )
451 mexWarnMsgTxt( "Options might be set incorrectly as struct has wrong number of entries!\n Type 'help qpOASES_options' for further information." );
452
453
454 if ( hasOptionsValue( optionsPtr,"maxIter",&optionValue ) == BT_TRUE )
455 if ( *optionValue >= 0.0 )
456 nWSRin = (int_t)*optionValue;
457
458 if ( hasOptionsValue( optionsPtr,"maxCpuTime",&optionValue ) == BT_TRUE )
459 if ( *optionValue >= 0.0 )
460 maxCpuTime = *optionValue;
461
462 if ( hasOptionsValue( optionsPtr,"printLevel",&optionValue ) == BT_TRUE )
463 {
464 #ifdef __SUPPRESSANYOUTPUT__
465 options->printLevel = PL_NONE;
466 #else
467 optionValueInt = (int_t)*optionValue;
468 options->printLevel = (REFER_NAMESPACE_QPOASES PrintLevel)optionValueInt;
469 if ( options->printLevel < PL_DEBUG_ITER )
470 options->printLevel = PL_DEBUG_ITER;
471 if ( options->printLevel > PL_HIGH )
472 options->printLevel = PL_HIGH;
473 #endif
474 }
475
476 if ( hasOptionsValue( optionsPtr,"enableRamping",&optionValue ) == BT_TRUE )
477 {
478 optionValueInt = (int_t)*optionValue;
479 options->enableRamping = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
480 }
481
482 if ( hasOptionsValue( optionsPtr,"enableFarBounds",&optionValue ) == BT_TRUE )
483 {
484 optionValueInt = (int_t)*optionValue;
485 options->enableFarBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
486 }
487
488 if ( hasOptionsValue( optionsPtr,"enableFlippingBounds",&optionValue ) == BT_TRUE )
489 {
490 optionValueInt = (int_t)*optionValue;
491 options->enableFlippingBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
492 }
493
494 if ( hasOptionsValue( optionsPtr,"enableRegularisation",&optionValue ) == BT_TRUE )
495 {
496 optionValueInt = (int_t)*optionValue;
497 options->enableRegularisation = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
498 }
499
500 if ( hasOptionsValue( optionsPtr,"enableFullLITests",&optionValue ) == BT_TRUE )
501 {
502 optionValueInt = (int_t)*optionValue;
503 options->enableFullLITests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
504 }
505
506 if ( hasOptionsValue( optionsPtr,"enableNZCTests",&optionValue ) == BT_TRUE )
507 {
508 optionValueInt = (int_t)*optionValue;
509 options->enableNZCTests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
510 }
511
512 if ( hasOptionsValue( optionsPtr,"enableDriftCorrection",&optionValue ) == BT_TRUE )
513 options->enableDriftCorrection = (int_t)*optionValue;
514
515 if ( hasOptionsValue( optionsPtr,"enableCholeskyRefactorisation",&optionValue ) == BT_TRUE )
516 options->enableCholeskyRefactorisation = (int_t)*optionValue;
517
518 if ( hasOptionsValue( optionsPtr,"enableEqualities",&optionValue ) == BT_TRUE )
519 {
520 optionValueInt = (int_t)*optionValue;
521 options->enableEqualities = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
522 }
523
524
525 if ( hasOptionsValue( optionsPtr,"terminationTolerance",&optionValue ) == BT_TRUE )
526 options->terminationTolerance = *optionValue;
527
528 if ( hasOptionsValue( optionsPtr,"boundTolerance",&optionValue ) == BT_TRUE )
529 options->boundTolerance = *optionValue;
530
531 if ( hasOptionsValue( optionsPtr,"boundRelaxation",&optionValue ) == BT_TRUE )
532 options->boundRelaxation = *optionValue;
533
534 if ( hasOptionsValue( optionsPtr,"epsNum",&optionValue ) == BT_TRUE )
535 options->epsNum = *optionValue;
536
537 if ( hasOptionsValue( optionsPtr,"epsDen",&optionValue ) == BT_TRUE )
538 options->epsDen = *optionValue;
539
540 if ( hasOptionsValue( optionsPtr,"maxPrimalJump",&optionValue ) == BT_TRUE )
541 options->maxPrimalJump = *optionValue;
542
543 if ( hasOptionsValue( optionsPtr,"maxDualJump",&optionValue ) == BT_TRUE )
544 options->maxDualJump = *optionValue;
545
546
547 if ( hasOptionsValue( optionsPtr,"initialRamping",&optionValue ) == BT_TRUE )
548 options->initialRamping = *optionValue;
549
550 if ( hasOptionsValue( optionsPtr,"finalRamping",&optionValue ) == BT_TRUE )
551 options->finalRamping = *optionValue;
552
553 if ( hasOptionsValue( optionsPtr,"initialFarBounds",&optionValue ) == BT_TRUE )
554 options->initialFarBounds = *optionValue;
555
556 if ( hasOptionsValue( optionsPtr,"growFarBounds",&optionValue ) == BT_TRUE )
557 options->growFarBounds = *optionValue;
558
559 if ( hasOptionsValue( optionsPtr,"initialStatusBounds",&optionValue ) == BT_TRUE )
560 {
561 optionValueInt = (int_t)*optionValue;
562 if ( optionValueInt < -1 )
563 optionValueInt = -1;
564 if ( optionValueInt > 1 )
565 optionValueInt = 1;
566 options->initialStatusBounds = (REFER_NAMESPACE_QPOASES SubjectToStatus)optionValueInt;
567 }
568
569 if ( hasOptionsValue( optionsPtr,"epsFlipping",&optionValue ) == BT_TRUE )
570 options->epsFlipping = *optionValue;
571
572 if ( hasOptionsValue( optionsPtr,"numRegularisationSteps",&optionValue ) == BT_TRUE )
573 options->numRegularisationSteps = (int_t)*optionValue;
574
575 if ( hasOptionsValue( optionsPtr,"epsRegularisation",&optionValue ) == BT_TRUE )
576 options->epsRegularisation = *optionValue;
577
578 if ( hasOptionsValue( optionsPtr,"numRefinementSteps",&optionValue ) == BT_TRUE )
579 options->numRefinementSteps = (int_t)*optionValue;
580
581 if ( hasOptionsValue( optionsPtr,"epsIterRef",&optionValue ) == BT_TRUE )
582 options->epsIterRef = *optionValue;
583
584 if ( hasOptionsValue( optionsPtr,"epsLITests",&optionValue ) == BT_TRUE )
585 options->epsLITests = *optionValue;
586
587 if ( hasOptionsValue( optionsPtr,"epsNZCTests",&optionValue ) == BT_TRUE )
588 options->epsNZCTests = *optionValue;
589
590 return SUCCESSFUL_RETURN;
591 }
592
593
594
595 /*
596 * s e t u p A u x i l i a r y I n p u t s
597 */
setupAuxiliaryInputs(const mxArray * auxInput,uint_t nV,uint_t nC,HessianType * hessianType,double ** x0,double ** guessedBounds,double ** guessedConstraints,double ** R)598 returnValue setupAuxiliaryInputs( const mxArray* auxInput, uint_t nV, uint_t nC,
599 HessianType* hessianType, double** x0, double** guessedBounds, double** guessedConstraints, double** R
600 )
601 {
602 mxArray* curField = 0;
603
604 /* hessianType */
605 curField = mxGetField( auxInput,0,"hessianType" );
606 if ( curField == NULL )
607 mexWarnMsgTxt( "auxInput struct does not contain entry 'hessianType'!\n Type 'help qpOASES_auxInput' for further information." );
608 else
609 {
610 if ( mxIsEmpty(curField) == true )
611 {
612 *hessianType = HST_UNKNOWN;
613 }
614 else
615 {
616 if ( mxIsScalar(curField) == false )
617 return RET_INVALID_ARGUMENTS;
618
619 double* hessianTypeTmp = mxGetPr(curField);
620 int_t hessianTypeInt = (int_t)*hessianTypeTmp;
621 if ( hessianTypeInt < 0 )
622 hessianTypeInt = 6; /* == HST_UNKNOWN */
623 if ( hessianTypeInt > 5 )
624 hessianTypeInt = 6; /* == HST_UNKNOWN */
625 *hessianType = (REFER_NAMESPACE_QPOASES HessianType)hessianTypeInt;
626 }
627 }
628
629 /* x0 */
630 curField = mxGetField( auxInput,0,"x0" );
631 if ( curField == NULL )
632 mexWarnMsgTxt( "auxInput struct does not contain entry 'x0'!\n Type 'help qpOASES_auxInput' for further information." );
633 else
634 {
635 *x0 = mxGetPr(curField);
636 if ( smartDimensionCheck( x0,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
637 return RET_INVALID_ARGUMENTS;
638 }
639
640 /* guessedWorkingSetB */
641 curField = mxGetField( auxInput,0,"guessedWorkingSetB" );
642 if ( curField == NULL )
643 mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetB'!\n Type 'help qpOASES_auxInput' for further information." );
644 else
645 {
646 *guessedBounds = mxGetPr(curField);
647 if ( smartDimensionCheck( guessedBounds,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
648 return RET_INVALID_ARGUMENTS;
649 }
650
651 /* guessedWorkingSetC */
652 curField = mxGetField( auxInput,0,"guessedWorkingSetC" );
653 if ( curField == NULL )
654 mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetC'!\n Type 'help qpOASES_auxInput' for further information." );
655 else
656 {
657 *guessedConstraints = mxGetPr(curField);
658 if ( smartDimensionCheck( guessedConstraints,nC,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
659 return RET_INVALID_ARGUMENTS;
660 }
661
662 /* R */
663 curField = mxGetField( auxInput,0,"R" );
664 if ( curField == NULL )
665 mexWarnMsgTxt( "auxInput struct does not contain entry 'R'!\n Type 'help qpOASES_auxInput' for further information." );
666 else
667 {
668 *R = mxGetPr(curField);
669 if ( smartDimensionCheck( R,nV,nV, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
670 return RET_INVALID_ARGUMENTS;
671 }
672
673 return SUCCESSFUL_RETURN;
674 }
675
676
677
678 /*
679 * a l l o c a t e O u t p u t s
680 */
allocateOutputs(int nlhs,mxArray * plhs[],int_t nV,int_t nC=0,int_t nP=1,int_t handle=-1)681 returnValue allocateOutputs( int nlhs, mxArray* plhs[], int_t nV, int_t nC = 0, int_t nP = 1, int_t handle = -1
682 )
683 {
684 /* Create output vectors and assign pointers to them. */
685 int_t curIdx = 0;
686
687 /* handle */
688 if ( handle >= 0 )
689 plhs[curIdx++] = mxCreateDoubleMatrix( 1, 1, mxREAL );
690
691 /* x */
692 plhs[curIdx++] = mxCreateDoubleMatrix( nV, nP, mxREAL );
693
694 if ( nlhs > curIdx )
695 {
696 /* fval */
697 plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
698
699 if ( nlhs > curIdx )
700 {
701 /* exitflag */
702 plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
703
704 if ( nlhs > curIdx )
705 {
706 /* iter */
707 plhs[curIdx++] = mxCreateDoubleMatrix( 1, nP, mxREAL );
708
709 if ( nlhs > curIdx )
710 {
711 /* lambda */
712 plhs[curIdx++] = mxCreateDoubleMatrix( nV+nC, nP, mxREAL );
713
714 if ( nlhs > curIdx )
715 {
716 /* setup auxiliary output struct */
717 mxArray* auxOutput = mxCreateStructMatrix( 1,1,0,0 );
718 int_t curFieldNum;
719
720 /* working set */
721 curFieldNum = mxAddField( auxOutput,"workingSetB" );
722 if ( curFieldNum >= 0 )
723 mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( nV, nP, mxREAL ) );
724
725 curFieldNum = mxAddField( auxOutput,"workingSetC" );
726 if ( curFieldNum >= 0 )
727 mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( nC, nP, mxREAL ) );
728
729 curFieldNum = mxAddField( auxOutput,"cpuTime" );
730 if ( curFieldNum >= 0 )
731 mxSetFieldByNumber( auxOutput,0,curFieldNum,mxCreateDoubleMatrix( 1, nP, mxREAL ) );
732
733 plhs[curIdx] = auxOutput;
734 }
735 }
736 }
737 }
738 }
739
740 return SUCCESSFUL_RETURN;
741 }
742
743
744 /*
745 * o b t a i n O u t p u t s
746 */
obtainOutputs(int_t k,QProblemB * qp,returnValue returnvalue,int_t _nWSRout,double _cpuTime,int nlhs,mxArray * plhs[],int_t nV,int_t nC=0,int_t handle=-1)747 returnValue obtainOutputs( int_t k, QProblemB* qp, returnValue returnvalue, int_t _nWSRout, double _cpuTime,
748 int nlhs, mxArray* plhs[], int_t nV, int_t nC = 0, int_t handle = -1
749 )
750 {
751 /* Create output vectors and assign pointers to them. */
752 int_t curIdx = 0;
753
754 /* handle */
755 if ( handle >= 0 )
756 plhs[curIdx++] = mxCreateDoubleScalar( handle );
757
758 /* x */
759 double* x = mxGetPr( plhs[curIdx++] );
760 qp->getPrimalSolution( &(x[k*nV]) );
761
762 if ( nlhs > curIdx )
763 {
764 /* fval */
765 double* obj = mxGetPr( plhs[curIdx++] );
766 obj[k] = qp->getObjVal( );
767
768 if ( nlhs > curIdx )
769 {
770 /* exitflag */
771 double* status = mxGetPr( plhs[curIdx++] );
772 status[k] = (double)getSimpleStatus( returnvalue );
773
774 if ( nlhs > curIdx )
775 {
776 /* iter */
777 double* nWSRout = mxGetPr( plhs[curIdx++] );
778 nWSRout[k] = (double) _nWSRout;
779
780 if ( nlhs > curIdx )
781 {
782 /* lambda */
783 double* y = mxGetPr( plhs[curIdx++] );
784 qp->getDualSolution( &(y[k*(nV+nC)]) );
785
786 /* auxOutput */
787 if ( nlhs > curIdx )
788 {
789 QProblem* problemPointer;
790 problemPointer = dynamic_cast<QProblem*>(qp);
791
792 mxArray* auxOutput = plhs[curIdx];
793 mxArray* curField = 0;
794
795 /* working set bounds */
796 if ( nV > 0 )
797 {
798 curField = mxGetField( auxOutput,0,"workingSetB" );
799 double* workingSetB = mxGetPr(curField);
800
801 /* cast successful? */
802 if (problemPointer != NULL) {
803 problemPointer->getWorkingSetBounds( &(workingSetB[k*nV]) );
804 } else {
805 qp->getWorkingSetBounds( &(workingSetB[k*nV]) );
806 }
807 }
808
809 /* working set constraints */
810 if ( nC > 0 )
811 {
812 curField = mxGetField( auxOutput,0,"workingSetC" );
813 double* workingSetC = mxGetPr(curField);
814
815 /* cast successful? */
816 if (problemPointer != NULL) {
817 problemPointer->getWorkingSetConstraints( &(workingSetC[k*nC]) );
818 } else {
819 qp->getWorkingSetConstraints( &(workingSetC[k*nC]) );
820 }
821 }
822
823 /* cpu time */
824 curField = mxGetField( auxOutput,0,"cpuTime" );
825 double* cpuTime = mxGetPr(curField);
826 cpuTime[0] = (double) _cpuTime;
827 }
828 }
829 }
830 }
831 }
832
833 return SUCCESSFUL_RETURN;
834 }
835
836
837
838 /*
839 * s e t u p H e s s i a n M a t r i x
840 */
setupHessianMatrix(const mxArray * prhsH,int_t nV,SymmetricMatrix ** H,sparse_int_t ** Hir,sparse_int_t ** Hjc,real_t ** Hv)841 returnValue setupHessianMatrix( const mxArray* prhsH, int_t nV,
842 SymmetricMatrix** H, sparse_int_t** Hir, sparse_int_t** Hjc, real_t** Hv
843 )
844 {
845 if ( prhsH == 0 )
846 return SUCCESSFUL_RETURN;
847
848 if ( mxIsSparse( prhsH ) != 0 )
849 {
850 mwIndex *mat_ir = mxGetIr( prhsH );
851 mwIndex *mat_jc = mxGetJc( prhsH );
852 double *v = (double*)mxGetPr( prhsH );
853 sparse_int_t nfill = 0;
854 mwIndex i, j;
855 BooleanType needInsertDiag;
856
857 /* copy indices to avoid 64/32-bit integer confusion */
858 /* also add explicit zeros on diagonal for regularization strategy */
859 /* copy values, too */
860 *Hir = new sparse_int_t[mat_jc[nV] + nV];
861 *Hjc = new sparse_int_t[nV+1];
862 *Hv = new real_t[mat_jc[nV] + nV];
863 for (j = 0; j < nV; j++)
864 {
865 needInsertDiag = BT_TRUE;
866
867 (*Hjc)[j] = (sparse_int_t)(mat_jc[j]) + nfill;
868 /* fill up to diagonal */
869 for (i = mat_jc[j]; i < mat_jc[j+1]; i++)
870 {
871 if ( mat_ir[i] == j )
872 needInsertDiag = BT_FALSE;
873
874 /* add zero diagonal element if not present */
875 if ( ( mat_ir[i] > j ) && ( needInsertDiag == BT_TRUE ) )
876 {
877 (*Hir)[i + nfill] = (sparse_int_t)j;
878 (*Hv)[i + nfill] = 0.0;
879 nfill++;
880 /* only add diag once */
881 needInsertDiag = BT_FALSE;
882 }
883
884 (*Hir)[i + nfill] = (sparse_int_t)(mat_ir[i]);
885 (*Hv)[i + nfill] = (real_t)(v[i]);
886 }
887 }
888 (*Hjc)[nV] = (sparse_int_t)(mat_jc[nV]) + nfill;
889
890 SymSparseMat *sH;
891 *H = sH = new SymSparseMat(nV, nV, *Hir, *Hjc, *Hv);
892 sH->createDiagInfo();
893 }
894 else
895 {
896 /* make a deep-copy in order to avoid modifying input data when regularising */
897 real_t* H_for = (real_t*) mxGetPr( prhsH );
898 real_t* H_mem = new real_t[nV*nV];
899 memcpy( H_mem,H_for, nV*nV*sizeof(real_t) );
900
901 *H = new SymDenseMat( nV,nV,nV, H_mem );
902 (*H)->doFreeMemory( );
903 }
904
905 return SUCCESSFUL_RETURN;
906 }
907
908
909 /*
910 * s e t u p C o n s t r a i n t M a t r i x
911 */
setupConstraintMatrix(const mxArray * prhsA,int_t nV,int_t nC,Matrix ** A,sparse_int_t ** Air,sparse_int_t ** Ajc,real_t ** Av)912 returnValue setupConstraintMatrix( const mxArray* prhsA, int_t nV, int_t nC,
913 Matrix** A, sparse_int_t** Air, sparse_int_t** Ajc, real_t** Av
914 )
915 {
916 if ( prhsA == 0 )
917 return SUCCESSFUL_RETURN;
918
919 if ( mxIsSparse( prhsA ) != 0 )
920 {
921 mwIndex i;
922 long j;
923
924 mwIndex *mat_ir = mxGetIr( prhsA );
925 mwIndex *mat_jc = mxGetJc( prhsA );
926 double *v = (double*)mxGetPr( prhsA );
927
928 /* copy indices to avoid 64/32-bit integer confusion */
929 *Air = new sparse_int_t[mat_jc[nV]];
930 *Ajc = new sparse_int_t[nV+1];
931 for (i = 0; i < mat_jc[nV]; i++)
932 (*Air)[i] = (sparse_int_t)(mat_ir[i]);
933 for (i = 0; i < nV + 1; i++)
934 (*Ajc)[i] = (sparse_int_t)(mat_jc[i]);
935
936 /* copy values, too */
937 *Av = new real_t[(*Ajc)[nV]];
938 for (j = 0; j < (*Ajc)[nV]; j++)
939 (*Av)[j] = (real_t)(v[j]);
940
941 *A = new SparseMatrix(nC, nV, *Air, *Ajc, *Av);
942 }
943 else
944 {
945 /* Convert constraint matrix A from FORTRAN to C style
946 * (not necessary for H as it should be symmetric!). */
947 real_t* A_for = (real_t*) mxGetPr( prhsA );
948 real_t* A_mem = new real_t[nC*nV];
949 convertFortranToC( A_for,nV,nC, A_mem );
950 *A = new DenseMatrix(nC, nV, nV, A_mem );
951 (*A)->doFreeMemory();
952 }
953
954 return SUCCESSFUL_RETURN;
955 }
956
957
958 /*
959 * end of file
960 */
961