00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00034 #include <acado/constraint/constraint_element.hpp>
00035
00036
00037 BEGIN_NAMESPACE_ACADO
00038
00039
00040
00041
00042
00043
00044
00045 ConstraintElement::ConstraintElement( ){
00046
00047 fcn = 0;
00048 lb = 0;
00049 ub = 0;
00050 y_index = 0;
00051 t_index = 0;
00052
00053 z = 0;
00054 JJ = 0;
00055
00056 nx = 0;
00057 na = 0;
00058 nu = 0;
00059 np = 0;
00060 nw = 0;
00061 ny = 0;
00062 nFcn = 0;
00063 nB = 0;
00064
00065 xSeed = 0 ;
00066 xaSeed = 0 ;
00067 pSeed = 0 ;
00068 uSeed = 0 ;
00069 wSeed = 0 ;
00070 bSeed = 0 ;
00071 xSeed2 = 0 ;
00072 xaSeed2 = 0 ;
00073 pSeed2 = 0 ;
00074 uSeed2 = 0 ;
00075 wSeed2 = 0 ;
00076 bSeed2 = 0 ;
00077
00078 condType = CT_SPARSE;
00079 }
00080
00081
00082 ConstraintElement::ConstraintElement( const Grid& grid_, int nFcn_, int nB_ ){
00083
00084 int run1;
00085
00086 grid = grid_;
00087 nFcn = nFcn_;
00088 nB = nB_ ;
00089
00090 fcn = new Function[nFcn];
00091 lb = new double* [ nB];
00092 ub = new double* [ nB];
00093
00094 for( run1 = 0; run1 < nB; run1++ ){
00095 lb[run1] = 0;
00096 ub[run1] = 0;
00097 }
00098
00099 y_index = new int* [nFcn];
00100 t_index = new int [nFcn];
00101
00102 for( run1 = 0; run1 < nFcn; run1++ ){
00103 y_index[run1] = 0;
00104 }
00105
00106 z = 0;
00107 JJ = 0;
00108
00109 nx = 0;
00110 na = 0;
00111 nu = 0;
00112 np = 0;
00113 nw = 0;
00114 ny = 0;
00115
00116 xSeed = 0 ;
00117 xaSeed = 0 ;
00118 pSeed = 0 ;
00119 uSeed = 0 ;
00120 wSeed = 0 ;
00121 bSeed = 0 ;
00122 xSeed2 = 0 ;
00123 xaSeed2 = 0 ;
00124 pSeed2 = 0 ;
00125 uSeed2 = 0 ;
00126 wSeed2 = 0 ;
00127 bSeed2 = 0 ;
00128
00129 condType = CT_SPARSE;
00130 }
00131
00132
00133 ConstraintElement::ConstraintElement( const ConstraintElement& rhs ){
00134
00135 int run1, run2;
00136
00137 grid = rhs.grid;
00138 nFcn = rhs.nFcn;
00139 nB = rhs.nB ;
00140
00141 if( rhs.fcn != 0 ) fcn = new Function[nFcn];
00142 else fcn = 0 ;
00143
00144 if( rhs.lb != 0 ) lb = new double* [nB ];
00145 else lb = 0 ;
00146
00147 if( rhs.ub != 0 ) ub = new double* [nB ];
00148 else ub = 0 ;
00149
00150 if( rhs.y_index != 0 ) y_index = new int* [nFcn];
00151 else y_index = 0 ;
00152
00153 if( rhs.t_index != 0 ) t_index = new int [nFcn];
00154 else t_index = 0 ;
00155
00156 if( rhs.z != 0 ) z = new EvaluationPoint( *(rhs.z) );
00157 else z = 0 ;
00158
00159 if( rhs.JJ != 0 ) JJ = new EvaluationPoint( *(rhs.JJ) );
00160 else JJ = 0 ;
00161
00162 nx = rhs.nx;
00163 na = rhs.na;
00164 nu = rhs.nu;
00165 np = rhs.np;
00166 nw = rhs.nw;
00167 ny = rhs.ny;
00168
00169 for( run1 = 0; run1 < nFcn; run1++ ){
00170
00171 fcn[run1] = rhs.fcn[run1];
00172
00173 if( ny > 0 ){
00174 y_index[run1] = new int[ny];
00175 for( run2 = 0; run2 < ny; run2++ )
00176 y_index[run1][run2] = rhs.y_index[run1][run2];
00177 }
00178 else{
00179 y_index[run1] = 0;
00180 }
00181
00182 t_index[run1] = rhs.t_index[run1];
00183 }
00184
00185 if( fcn == 0 ){
00186 for( run1 = 0; run1 < nB; run1++ ){
00187 lb[run1] = 0;
00188 ub[run1] = 0;
00189 }
00190 }
00191 else{
00192 for( run1 = 0; run1 < nB; run1++ ){
00193
00194 if( fcn[0].getDim() > 0 ){
00195 lb[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
00196 ub[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
00197 }
00198 else{
00199 lb[run1] = 0;
00200 ub[run1] = 0;
00201 }
00202
00203 for( run2 = 0; run2 < fcn[0].getDim(); run2++ ){
00204 lb[run1][run2] = rhs.lb[run1][run2];
00205 ub[run1][run2] = rhs.ub[run1][run2];
00206 }
00207 }
00208 }
00209
00210 if( rhs.xSeed != 0 ) xSeed = new BlockMatrix(*rhs.xSeed );
00211 else xSeed = 0 ;
00212 if( rhs.xaSeed != 0 ) xaSeed = new BlockMatrix(*rhs.xaSeed);
00213 else xaSeed = 0 ;
00214 if( rhs.pSeed != 0 ) pSeed = new BlockMatrix(*rhs.pSeed) ;
00215 else pSeed = 0 ;
00216 if( rhs.uSeed != 0 ) uSeed = new BlockMatrix(*rhs.uSeed) ;
00217 else uSeed = 0 ;
00218 if( rhs.wSeed != 0 ) wSeed = new BlockMatrix(*rhs.wSeed) ;
00219 else wSeed = 0 ;
00220
00221 if( rhs.bSeed != 0 ) bSeed = new BlockMatrix(*rhs.bSeed );
00222 else bSeed = 0 ;
00223
00224 if( rhs.xSeed2 != 0 ) xSeed2 = new BlockMatrix(*rhs.xSeed2) ;
00225 else xSeed2 = 0 ;
00226 if( rhs.xaSeed2 != 0 ) xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
00227 else xaSeed2 = 0 ;
00228 if( rhs.pSeed2 != 0 ) pSeed2 = new BlockMatrix(*rhs.pSeed2 );
00229 else pSeed2 = 0 ;
00230 if( rhs.uSeed2 != 0 ) uSeed2 = new BlockMatrix(*rhs.uSeed2 );
00231 else uSeed2 = 0 ;
00232 if( rhs.wSeed2 != 0 ) wSeed2 = new BlockMatrix(*rhs.wSeed2 );
00233 else wSeed2 = 0 ;
00234
00235 if( rhs.bSeed != 0 ) bSeed2 = new BlockMatrix(*rhs.bSeed );
00236 else bSeed2 = 0 ;
00237
00238
00239 residuumL = rhs.residuumL;
00240 residuumU = rhs.residuumU;
00241 dForward = rhs.dForward ;
00242 dBackward = rhs.dBackward;
00243
00244 condType = rhs.condType;
00245 }
00246
00247
00248 ConstraintElement::~ConstraintElement( ){
00249
00250 int run1;
00251
00252 if( fcn != 0 )
00253 delete[] fcn;
00254
00255 for( run1 = 0; run1 < nB; run1++ ){
00256
00257 if( lb[run1] != 0 )
00258 free(lb[run1]);
00259
00260 if( ub[run1] != 0 )
00261 free(ub[run1]);
00262 }
00263
00264 if( lb != 0 )
00265 delete[] lb;
00266
00267 if( ub != 0 )
00268 delete[] ub;
00269
00270 for( run1 = 0; run1 < nFcn; run1++ ){
00271
00272 if( y_index[run1] != 0 )
00273 delete[] y_index[run1];
00274 }
00275
00276 if( y_index != 0 )
00277 delete[] y_index;
00278
00279 if( t_index != 0 )
00280 delete[] t_index;
00281
00282 if ( z != 0 )
00283 delete[] z;
00284
00285 if ( JJ != 0 )
00286 delete[] JJ;
00287
00288 if( xSeed != 0 ) delete xSeed ;
00289 if( xaSeed != 0 ) delete xaSeed ;
00290 if( pSeed != 0 ) delete pSeed ;
00291 if( uSeed != 0 ) delete uSeed ;
00292 if( wSeed != 0 ) delete wSeed ;
00293
00294 if( bSeed != 0 ) delete bSeed ;
00295
00296 if( xSeed2 != 0 ) delete xSeed2 ;
00297 if( xaSeed2 != 0 ) delete xaSeed2;
00298 if( pSeed2 != 0 ) delete pSeed2 ;
00299 if( uSeed2 != 0 ) delete uSeed2 ;
00300 if( wSeed2 != 0 ) delete wSeed2 ;
00301
00302 if( bSeed2 != 0 ) delete bSeed2 ;
00303 }
00304
00305
00306 ConstraintElement& ConstraintElement::operator=( const ConstraintElement& rhs ){
00307
00308 int run1, run2;
00309
00310 if( this != &rhs ){
00311
00312 if( fcn != 0 )
00313 delete[] fcn;
00314
00315 for( run1 = 0; run1 < nB; run1++ ){
00316
00317 if( lb[run1] != 0 )
00318 free(lb[run1]);
00319
00320 if( ub[run1] != 0 )
00321 free(ub[run1]);
00322 }
00323
00324 if( lb != 0 )
00325 delete[] lb;
00326
00327 if( ub != 0 )
00328 delete[] ub;
00329
00330 for( run1 = 0; run1 < nFcn; run1++ ){
00331
00332 if( y_index[run1] != 0 )
00333 delete[] y_index[run1];
00334 }
00335
00336 if( y_index != 0 )
00337 delete[] y_index;
00338
00339 if( t_index != 0 )
00340 delete[] t_index;
00341
00342 if ( z != 0 )
00343 delete[] z;
00344
00345 if ( JJ != 0 )
00346 delete[] JJ;
00347
00348 if( xSeed != 0 ) delete xSeed ;
00349 if( xaSeed != 0 ) delete xaSeed ;
00350 if( pSeed != 0 ) delete pSeed ;
00351 if( uSeed != 0 ) delete uSeed ;
00352 if( wSeed != 0 ) delete wSeed ;
00353
00354 if( bSeed != 0 ) delete bSeed ;
00355
00356 if( xSeed2 != 0 ) delete xSeed2 ;
00357 if( xaSeed2 != 0 ) delete xaSeed2;
00358 if( pSeed2 != 0 ) delete pSeed2 ;
00359 if( uSeed2 != 0 ) delete uSeed2 ;
00360 if( wSeed2 != 0 ) delete wSeed2 ;
00361
00362 if( bSeed2 != 0 ) delete bSeed2 ;
00363
00364
00365 grid = rhs.grid;
00366 nFcn = rhs.nFcn;
00367 nB = rhs.nB ;
00368
00369 if( rhs.fcn != 0 ) fcn = new Function[nFcn];
00370 else fcn = 0 ;
00371
00372 if( rhs.lb != 0 ) lb = new double* [nB ];
00373 else lb = 0 ;
00374
00375 if( rhs.ub != 0 ) ub = new double* [nB ];
00376 else ub = 0 ;
00377
00378 if( rhs.y_index != 0 ) y_index = new int* [nFcn];
00379 else y_index = 0 ;
00380
00381 if( rhs.t_index != 0 ) t_index = new int [nFcn];
00382 else t_index = 0 ;
00383
00384 if( rhs.z != 0 ) z = new EvaluationPoint( *(rhs.z) );
00385 else z = 0 ;
00386
00387 if( rhs.JJ != 0 ) JJ = new EvaluationPoint( *(rhs.JJ) );
00388 else JJ = 0 ;
00389
00390 nx = rhs.nx;
00391 na = rhs.na;
00392 nu = rhs.nu;
00393 np = rhs.np;
00394 nw = rhs.nw;
00395 ny = rhs.ny;
00396
00397 for( run1 = 0; run1 < nFcn; run1++ ){
00398
00399 fcn[run1] = rhs.fcn[run1];
00400
00401 if( ny > 0 ){
00402 y_index[run1] = new int[ny];
00403 for( run2 = 0; run2 < ny; run2++ )
00404 y_index[run1][run2] = rhs.y_index[run1][run2];
00405 }
00406 else{
00407 y_index[run1] = 0;
00408 }
00409
00410 t_index[run1] = rhs.t_index[run1];
00411
00412 }
00413
00414 if( fcn == 0 ){
00415 for( run1 = 0; run1 < nB; run1++ ){
00416 lb[run1] = 0;
00417 ub[run1] = 0;
00418 }
00419 }
00420 else{
00421 for( run1 = 0; run1 < nB; run1++ ){
00422
00423 if( fcn[0].getDim() > 0 ){
00424 lb[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
00425 ub[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
00426 }
00427 else{
00428 lb[run1] = 0;
00429 ub[run1] = 0;
00430 }
00431
00432 for( run2 = 0; run2 < fcn[0].getDim(); run2++ ){
00433 lb[run1][run2] = rhs.lb[run1][run2];
00434 ub[run1][run2] = rhs.ub[run1][run2];
00435 }
00436 }
00437 }
00438
00439 if( rhs.xSeed != 0 ) xSeed = new BlockMatrix(*rhs.xSeed );
00440 else xSeed = 0 ;
00441 if( rhs.xaSeed != 0 ) xaSeed = new BlockMatrix(*rhs.xaSeed );
00442 else xaSeed = 0 ;
00443 if( rhs.pSeed != 0 ) pSeed = new BlockMatrix(*rhs.pSeed );
00444 else pSeed = 0 ;
00445 if( rhs.uSeed != 0 ) uSeed = new BlockMatrix(*rhs.uSeed );
00446 else uSeed = 0 ;
00447 if( rhs.wSeed != 0 ) wSeed = new BlockMatrix(*rhs.wSeed );
00448 else wSeed = 0 ;
00449
00450 if( rhs.bSeed != 0 ) bSeed = new BlockMatrix(*rhs.bSeed );
00451 else bSeed = 0 ;
00452
00453 if( rhs.xSeed2 != 0 ) xSeed2 = new BlockMatrix(*rhs.xSeed2 );
00454 else xSeed2 = 0 ;
00455 if( rhs.xaSeed2 != 0 ) xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
00456 else xaSeed2 = 0 ;
00457 if( rhs.pSeed2 != 0 ) pSeed2 = new BlockMatrix(*rhs.pSeed2 );
00458 else pSeed2 = 0 ;
00459 if( rhs.uSeed2 != 0 ) uSeed2 = new BlockMatrix(*rhs.uSeed2 );
00460 else uSeed2 = 0 ;
00461 if( rhs.wSeed2 != 0 ) wSeed2 = new BlockMatrix(*rhs.wSeed2 );
00462 else wSeed2 = 0 ;
00463
00464 if( rhs.bSeed != 0 ) bSeed2 = new BlockMatrix(*rhs.bSeed );
00465 else bSeed2 = 0 ;
00466
00467 residuumL = rhs.residuumL;
00468 residuumU = rhs.residuumU;
00469 dForward = rhs.dForward ;
00470 dBackward = rhs.dBackward;
00471
00472 condType = rhs.condType;
00473 }
00474
00475 return *this;
00476 }
00477
00478
00479 returnValue ConstraintElement::init( const OCPiterate& iter ){
00480
00481 int run1, run2;
00482
00483 initializeEvaluationPoints( iter );
00484
00485
00486 if( iter.x != NULL ) nx = iter.x ->getNumValues();
00487 else nx = 0 ;
00488
00489 if( iter.xa != NULL ) na = iter.xa->getNumValues();
00490 else na = 0 ;
00491
00492 if( iter.p != NULL ) np = iter.p ->getNumValues();
00493 else np = 0 ;
00494
00495 if( iter.u != NULL ) nu = iter.u ->getNumValues();
00496 else nu = 0 ;
00497
00498 if( iter.w != NULL ) nw = iter.w ->getNumValues();
00499 else nw = 0 ;
00500
00501 ny = nx+na+nu+np+nw;
00502
00503 for( run2 = 0; run2 < nFcn; run2++ ){
00504
00505 if( y_index[run2] != 0 ) delete[] y_index[run2];
00506 y_index[run2] = new int[ny] ;
00507
00508 for( run1 = 0; run1 < nx; run1++ )
00509 y_index[run2][run1] = fcn[run2].index( VT_DIFFERENTIAL_STATE, run1 );
00510
00511 for( run1 = 0; run1 < na; run1++ )
00512 y_index[run2][nx+run1] = fcn[run2].index( VT_ALGEBRAIC_STATE, run1 );
00513
00514 for( run1 = 0; run1 < np; run1++ )
00515 y_index[run2][nx+na+run1] = fcn[run2].index( VT_PARAMETER, run1 );
00516
00517 for( run1 = 0; run1 < nu; run1++ )
00518 y_index[run2][nx+na+np+run1] = fcn[run2].index( VT_CONTROL, run1 );
00519
00520 for( run1 = 0; run1 < nw; run1++ )
00521 y_index[run2][nx+na+np+nu+run1] = fcn[run2].index( VT_DISTURBANCE, run1 );
00522
00523 t_index[run2] = fcn[run2].index( VT_TIME, 0 );
00524 }
00525
00526 return SUCCESSFUL_RETURN;
00527 }
00528
00529
00530 returnValue ConstraintElement::setForwardSeed( BlockMatrix *xSeed_,
00531 BlockMatrix *xaSeed_,
00532 BlockMatrix *pSeed_,
00533 BlockMatrix *uSeed_,
00534 BlockMatrix *wSeed_,
00535 int order ){
00536
00537 if( order == 1 ){
00538
00539 if( xSeed_ != 0 ){
00540 if( xSeed != 0 ) delete xSeed;
00541 xSeed = new BlockMatrix(*xSeed_);
00542 }
00543 else{
00544 if( xSeed != 0 ) delete xSeed;
00545 xSeed = 0;
00546 }
00547 if( xaSeed_ != 0 ){
00548 if( xaSeed != 0 ) delete xaSeed;
00549 xaSeed = new BlockMatrix(*xaSeed_);
00550 }
00551 else{
00552 if( xaSeed != 0 ) delete xaSeed;
00553 xaSeed = 0;
00554 }
00555 if( pSeed_ != 0 ){
00556 if( pSeed != 0 ) delete pSeed;
00557 pSeed = new BlockMatrix(*pSeed_);
00558 }
00559 else{
00560 if( pSeed != 0 ) delete pSeed;
00561 pSeed = 0;
00562 }
00563 if( uSeed_ != 0 ){
00564 if( uSeed != 0 ) delete uSeed;
00565 uSeed = new BlockMatrix(*uSeed_);
00566 }
00567 else{
00568 if( uSeed != 0 ) delete uSeed;
00569 uSeed = 0;
00570 }
00571 if( wSeed_ != 0 ){
00572 if( wSeed != 0 ) delete wSeed;
00573 wSeed = new BlockMatrix(*wSeed_);
00574 }
00575 else{
00576 if( wSeed != 0 ) delete wSeed;
00577 wSeed = 0;
00578 }
00579
00580 return SUCCESSFUL_RETURN;
00581 }
00582 if( order == 2 ){
00583
00584 if( xSeed_ != 0 ){
00585 if( xSeed2 != 0 ) delete xSeed2;
00586 xSeed2 = new BlockMatrix(*xSeed_);
00587 }
00588 else{
00589 if( xSeed2 != 0 ) delete xSeed2;
00590 xSeed2 = 0;
00591 }
00592 if( xaSeed_ != 0 ){
00593 if( xaSeed2 != 0 ) delete xaSeed2;
00594 xaSeed2 = new BlockMatrix(*xaSeed_);
00595 }
00596 else{
00597 if( xaSeed2 != 0 ) delete xaSeed2;
00598 xaSeed2 = 0;
00599 }
00600 if( pSeed_ != 0 ){
00601 if( pSeed2 != 0 ) delete pSeed2;
00602 pSeed2 = new BlockMatrix(*pSeed_);
00603 }
00604 else{
00605 if( pSeed2 != 0 ) delete pSeed2;
00606 pSeed2 = 0;
00607 }
00608 if( uSeed_ != 0 ){
00609 if( uSeed2 != 0 ) delete uSeed2;
00610 uSeed2 = new BlockMatrix(*uSeed_);
00611 }
00612 else{
00613 if( uSeed2 != 0 ) delete uSeed2;
00614 uSeed2 = 0;
00615 }
00616 if( wSeed_ != 0 ){
00617 if( wSeed2 != 0 ) delete wSeed2;
00618 wSeed2 = new BlockMatrix(*wSeed_);
00619 }
00620 else{
00621 if( wSeed2 != 0 ) delete wSeed2;
00622 wSeed2 = 0;
00623 }
00624 return SUCCESSFUL_RETURN;
00625 }
00626
00627 return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00628 }
00629
00630
00631 returnValue ConstraintElement::setUnitForwardSeed( ){
00632
00633 BlockMatrix xSeed_( 1, 1 );
00634 BlockMatrix xaSeed_( 1, 1 );
00635 BlockMatrix pSeed_( 1, 1 );
00636 BlockMatrix uSeed_( 1, 1 );
00637 BlockMatrix wSeed_( 1, 1 );
00638
00639 xSeed_.setIdentity ( 0, 0, nx );
00640 xaSeed_.setIdentity( 0, 0, na );
00641 pSeed_.setIdentity ( 0, 0, np );
00642 uSeed_.setIdentity ( 0, 0, nu );
00643 wSeed_.setIdentity ( 0, 0, nw );
00644
00645 return setForwardSeed( &xSeed_, &xaSeed_, &pSeed_, &uSeed_, &wSeed_, 1 );
00646 }
00647
00648
00649 returnValue ConstraintElement::setBackwardSeed( BlockMatrix *seed, int order ){
00650
00651 if( order == 1 ){
00652
00653 if( seed != 0 ){
00654 if( bSeed != 0 ) delete bSeed;
00655 bSeed = new BlockMatrix(*seed);
00656 }
00657 else{
00658 if( bSeed != 0 ) delete bSeed;
00659 bSeed = 0;
00660 }
00661
00662 return SUCCESSFUL_RETURN;
00663 }
00664 if( order == 2 ){
00665
00666 if( seed != 0 ){
00667 if( bSeed2 != 0 ) delete bSeed2;
00668 bSeed2 = new BlockMatrix(*seed);
00669 }
00670 else{
00671 if( bSeed2 != 0 ) delete bSeed2;
00672 bSeed2 = 0;
00673 }
00674 return SUCCESSFUL_RETURN;
00675 }
00676
00677 return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00678 }
00679
00680
00681 returnValue ConstraintElement::getResiduum( BlockMatrix &lower_residuum,
00682 BlockMatrix &upper_residuum ){
00683
00684
00685 lower_residuum = residuumL;
00686 upper_residuum = residuumU;
00687
00688 return SUCCESSFUL_RETURN;
00689 }
00690
00691
00692 returnValue ConstraintElement::getForwardSensitivities( BlockMatrix *D, int order ){
00693
00694 ASSERT( D != 0 );
00695
00696 if( order == 1 ){
00697
00698 D[0] = dForward;
00699 return SUCCESSFUL_RETURN;
00700 }
00701 if( order == 2 ){
00702
00703 return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00704 }
00705 return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00706 }
00707
00708
00709 returnValue ConstraintElement::getBackwardSensitivities( BlockMatrix *D, int order ){
00710
00711 ASSERT( D != 0 );
00712
00713 if( order == 1 ){
00714
00715 D[0] = dBackward;
00716 return SUCCESSFUL_RETURN;
00717 }
00718 if( order == 2 ){
00719
00720 return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00721 }
00722 return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00723 }
00724
00725
00726
00727
00728
00729
00730
00731 returnValue ConstraintElement::initializeEvaluationPoints( const OCPiterate& iter )
00732 {
00733 if ( z != 0 )
00734 delete[] z;
00735
00736 if ( JJ != 0 )
00737 delete[] JJ;
00738
00739 z = new EvaluationPoint[nFcn];
00740 JJ = new EvaluationPoint[nFcn];
00741
00742
00743 for( int i=0; i<nFcn; ++i )
00744 {
00745 z[i].init( fcn[i], iter );
00746 JJ[i].init( fcn[i], iter );
00747
00748 }
00749
00750 return SUCCESSFUL_RETURN;
00751 }
00752
00753 returnValue ConstraintElement::get(Function& function_, DMatrix& lb_, DMatrix& ub_)
00754 {
00755 if ( fcn == NULL )
00756 return RET_INITIALIZE_FIRST;
00757
00758
00759 function_ = fcn[ 0 ];
00760
00761 int dimFcn = fcn[ 0 ].getDim();
00762
00763 if ( dimFcn == 0 )
00764 {
00765 lb_.init(0, 0);
00766 ub_.init(0, 0);
00767
00768 return SUCCESSFUL_RETURN;
00769 }
00770
00771 lb_.init(nB, dimFcn);
00772 ub_.init(nB, dimFcn);
00773
00774 int i, j;
00775
00776 for (i = 0; i < nB; ++i)
00777 for (j = 0; j < dimFcn; ++j)
00778 {
00779 lb_(i, j) = lb[ i ][ j ];
00780 ub_(i, j) = ub[ i ][ j ];
00781 }
00782
00783 return SUCCESSFUL_RETURN;
00784 }
00785
00786
00787 CLOSE_NAMESPACE_ACADO
00788
00789