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/code_generation/integrators/integrator_export.hpp>
00035
00036
00037
00038 BEGIN_NAMESPACE_ACADO
00039
00040
00041
00042
00043
00044
00045 IntegratorExport::IntegratorExport( UserInteraction* _userInteraction,
00046 const std::string& _commonHeaderName
00047 ) : ExportAlgorithm( _userInteraction,_commonHeaderName )
00048 {
00049 NX1 = 0;
00050 NX2 = 0;
00051 NX3 = 0;
00052
00053 NDX3 = 0;
00054 NXA3 = 0;
00055
00056 timeDependant = false;
00057
00058 exportRhs = true;
00059 crsFormat = false;
00060
00061 reset_int = ExportVariable( "resetIntegrator", 1, 1, INT, ACADO_LOCAL, true );
00062 error_code = ExportVariable( "error", 1, 1, INT, ACADO_LOCAL, true );
00063 }
00064
00065
00066 IntegratorExport::IntegratorExport( const IntegratorExport& arg
00067 ) : ExportAlgorithm( arg )
00068 {
00069 NX1 = arg.NX1;
00070 NX2 = arg.NX2;
00071 NX3 = arg.NX3;
00072
00073 NDX3 = arg.NDX3;
00074 NXA3 = arg.NXA3;
00075
00076 timeDependant = false;
00077
00078 exportRhs = true;
00079 crsFormat = false;
00080 }
00081
00082
00083 IntegratorExport::~IntegratorExport( )
00084 {
00085 clear( );
00086 }
00087
00088
00089 IntegratorExport& IntegratorExport::operator=( const IntegratorExport& arg
00090 )
00091 {
00092 if( this != &arg && &arg != 0 )
00093 {
00094 clear( );
00095 ExportAlgorithm::operator=( arg );
00096 }
00097 return *this;
00098 }
00099
00100
00101 returnValue IntegratorExport::setGrid( const Grid& _grid )
00102 {
00103 grid = _grid;
00104
00105 return SUCCESSFUL_RETURN;
00106 }
00107
00108
00109 returnValue IntegratorExport::setLinearInput( const DMatrix& M1, const DMatrix& A1, const DMatrix& B1 )
00110 {
00111 if( !A1.isEmpty() ) {
00112 if( A1.getNumRows() != M1.getNumRows() || A1.getNumRows() != B1.getNumRows() || A1.getNumRows() != A1.getNumCols() || M1.getNumRows() != M1.getNumCols() || B1.getNumCols() != NU) {
00113 return RET_UNABLE_TO_EXPORT_CODE;
00114 }
00115 NX1 = A1.getNumRows();
00116 M11 = M1;
00117 A11 = A1;
00118 B11 = B1;
00119
00120 OnlineData dummy0;
00121 Control dummy1;
00122 DifferentialState dummy2;
00123 dummy0.clearStaticCounters();
00124 dummy1.clearStaticCounters();
00125 dummy2.clearStaticCounters();
00126 x = DifferentialState("", NX1, 1);
00127 u = Control("", NU, 1);
00128 od = OnlineData("", NOD, 1);
00129
00130 DifferentialEquation fun_input;
00131 fun_input << A11*x+B11*u;
00132 lin_input.init(fun_input, "acado_linear_input", NX, NXA, NU);
00133 }
00134
00135 return SUCCESSFUL_RETURN;
00136 }
00137
00138
00139 returnValue IntegratorExport::setModel( const std::string& _name_ODE, const std::string& _name_diffs_ODE ) {
00140
00141 if( rhs.getFunctionDim() == 0 ) {
00142 rhs = ExportAcadoFunction(_name_ODE);
00143 diffs_rhs = ExportAcadoFunction(_name_diffs_ODE);
00144
00145 exportRhs = false;
00146 }
00147 else {
00148 return ACADOERROR( RET_INVALID_OPTION );
00149 }
00150
00151 OnlineData dummy0;
00152 Control dummy1;
00153 DifferentialState dummy2;
00154 AlgebraicState dummy3;
00155 DifferentialStateDerivative dummy4;
00156 dummy0.clearStaticCounters();
00157 dummy1.clearStaticCounters();
00158 dummy2.clearStaticCounters();
00159 dummy3.clearStaticCounters();
00160 dummy4.clearStaticCounters();
00161
00162 NX2 = NX-NX1-NX3;
00163
00164 x = DifferentialState("", NX, 1);
00165 dx = DifferentialStateDerivative("", NDX, 1);
00166 z = AlgebraicState("", NXA, 1);
00167 u = Control("", NU, 1);
00168 od = OnlineData("", NOD, 1);
00169
00170 return SUCCESSFUL_RETURN;
00171 }
00172
00173
00174 returnValue IntegratorExport::setLinearOutput( const DMatrix& M3, const DMatrix& A3, const Expression& _rhs )
00175 {
00176 if( !A3.isEmpty() ) {
00177 if( A3.getNumRows() != M3.getNumRows() || M3.getNumRows() != M3.getNumCols() || A3.getNumRows() != A3.getNumCols() || A3.getNumRows() != _rhs.getDim() ) {
00178 return RET_UNABLE_TO_EXPORT_CODE;
00179 }
00180 NX3 = A3.getNumRows();
00181 M33 = M3;
00182 A33 = A3;
00183
00184 OutputFcn f;
00185 f << _rhs;
00186 OnlineData dummy0;
00187 Control dummy1;
00188 DifferentialState dummy2;
00189 AlgebraicState dummy3;
00190 DifferentialStateDerivative dummy4;
00191 dummy0.clearStaticCounters();
00192 dummy1.clearStaticCounters();
00193 dummy2.clearStaticCounters();
00194 x = DifferentialState("", NX1+NX2, 1);
00195 u = Control("", NU, 1);
00196 od = OnlineData("", NOD, 1);
00197
00198 if( (uint)f.getNX() > (NX1+NX2) ) {
00199 return ACADOERROR( RET_INVALID_LINEAR_OUTPUT_FUNCTION );
00200 }
00201 if( (uint)f.getNDX() > (NX1+NX2) ) {
00202 return ACADOERROR( RET_INVALID_LINEAR_OUTPUT_FUNCTION );
00203 }
00204 if( f.getNDX() > 0 ) {
00205 NDX3 = NX1+NX2;
00206 NDX = NX;
00207 }
00208 else NDX3 = 0;
00209
00210 dummy4.clearStaticCounters();
00211 dx = DifferentialStateDerivative("", NDX3, 1);
00212
00213 if( f.getNXA() > 0 && NXA == 0 ) {
00214 return ACADOERROR( RET_INVALID_LINEAR_OUTPUT_FUNCTION );
00215 }
00216 if( f.getNXA() > 0 ) NXA3 = NXA;
00217 else NXA3 = 0;
00218 dummy3.clearStaticCounters();
00219 z = AlgebraicState("", NXA3, 1);
00220
00221 uint i;
00222 OutputFcn g;
00223 for( i = 0; i < _rhs.getDim(); i++ ) {
00224 g << forwardDerivative( _rhs(i), x );
00225 g << forwardDerivative( _rhs(i), z );
00226 g << forwardDerivative( _rhs(i), u );
00227 g << forwardDerivative( _rhs(i), dx );
00228 }
00229
00230 dummy2.clearStaticCounters();
00231 x = DifferentialState("", NX, 1);
00232
00233 DMatrix dependencyMat = _rhs.getDependencyPattern( x );
00234 DVector dependency = dependencyMat.sumRow( );
00235 for( i = NX1+NX2; i < NX; i++ ) {
00236 if( acadoRoundAway(dependency(i)) != 0 ) {
00237 return RET_UNABLE_TO_EXPORT_CODE;
00238 }
00239 }
00240
00241 OutputFcn f_large;
00242 DMatrix A3_large = expandOutputMatrix(A3);
00243 f_large << _rhs + A3_large*x;
00244
00245 return (rhs3.init(f_large, "acado_rhs3", NX, NXA, NU, NP, NDX, NOD) &
00246 diffs_rhs3.init(g, "acado_diffs3", NX, NXA, NU, NP, NDX, NOD));
00247 }
00248
00249 return SUCCESSFUL_RETURN;
00250 }
00251
00252
00253 returnValue IntegratorExport::setLinearOutput( const DMatrix& M3, const DMatrix& A3, const std::string& _rhs3, const std::string& _diffs_rhs3 )
00254 {
00255 if( !A3.isEmpty() ) {
00256 if( A3.getNumRows() != M3.getNumRows() || M3.getNumRows() != M3.getNumCols() || A3.getNumRows() != A3.getNumCols() ) {
00257 return RET_UNABLE_TO_EXPORT_CODE;
00258 }
00259 NX3 = A3.getNumRows();
00260 M33 = M3;
00261 A33 = A3;
00262
00263 rhs3 = ExportAcadoFunction(_rhs3);
00264 diffs_rhs3 = ExportAcadoFunction(_diffs_rhs3);
00265 exportRhs = false;
00266
00267 OnlineData dummy0;
00268 Control dummy1;
00269 DifferentialState dummy2;
00270 AlgebraicState dummy3;
00271 DifferentialStateDerivative dummy4;
00272 dummy0.clearStaticCounters();
00273 dummy1.clearStaticCounters();
00274 dummy2.clearStaticCounters();
00275 dummy3.clearStaticCounters();
00276 dummy4.clearStaticCounters();
00277
00278 x = DifferentialState("", NX, 1);
00279 dx = DifferentialStateDerivative("", NDX, 1);
00280 z = AlgebraicState("", NXA, 1);
00281 u = Control("", NU, 1);
00282 od = OnlineData("", NOD, 1);
00283 }
00284
00285 return SUCCESSFUL_RETURN;
00286 }
00287
00288
00289 returnValue IntegratorExport::setModelData( const ModelData& data ) {
00290
00291 setDimensions( data.getNX(),data.getNDX(),data.getNXA(),data.getNU(),data.getNP(),data.getN(), data.getNOD() );
00292 NX1 = data.getNX1();
00293 NX2 = data.getNX2();
00294 NX3 = data.getNX3();
00295 NDX3 = data.getNDX3();
00296 NXA3 = data.getNXA3();
00297 exportRhs = data.exportRhs();
00298
00299 DMatrix M1, A1, B1;
00300 data.getLinearInput( M1, A1, B1 );
00301 if ( M1.getNumRows() > 0 && setLinearInput( M1, A1, B1 ) != SUCCESSFUL_RETURN )
00302 return RET_UNABLE_TO_EXPORT_CODE;
00303
00304 DMatrix M3, A3;
00305 data.getLinearOutput( M3, A3 );
00306
00307 if( exportRhs ) {
00308 DifferentialEquation f;
00309 data.getModel(f);
00310 OutputFcn f3;
00311 data.getLinearOutput( M3, A3, f3 );
00312 DMatrix parms;
00313 uint delay;
00314 data.getNARXmodel( delay, parms );
00315
00316 Expression rhs_, rhs3_;
00317 f.getExpression( rhs_ );
00318 f3.getExpression( rhs3_ );
00319
00320 if ( f.getDim() > 0 && setDifferentialEquation( rhs_ ) != SUCCESSFUL_RETURN )
00321 return RET_UNABLE_TO_EXPORT_CODE;
00322 if ( !parms.isEmpty() && setNARXmodel( delay, parms ) != SUCCESSFUL_RETURN )
00323 return RET_UNABLE_TO_EXPORT_CODE;
00324 if ( M3.getNumRows() > 0 && setLinearOutput( M3, A3, rhs3_ ) != SUCCESSFUL_RETURN )
00325 return RET_UNABLE_TO_EXPORT_CODE;
00326 }
00327 else {
00328 if ( M3.getNumRows() > 0 && setLinearOutput( M3, A3, data.getNameOutput(), data.getNameDiffsOutput() ) != SUCCESSFUL_RETURN )
00329 return RET_UNABLE_TO_EXPORT_CODE;
00330 if ( setModel( data.getNameRhs(), data.getNameDiffsRhs() ) != SUCCESSFUL_RETURN )
00331 return RET_UNABLE_TO_EXPORT_CODE;
00332 }
00333 Grid integrationGrid;
00334 data.getIntegrationGrid(integrationGrid);
00335 grid = integrationGrid;
00336 data.getNumSteps( numSteps );
00337
00338 setup( );
00339
00340 if( data.hasOutputs() ) {
00341 std::vector<Grid> outputGrids_;
00342 data.getOutputGrids(outputGrids_);
00343 std::vector<Expression> outputExpressions_;
00344 data.getOutputExpressions(outputExpressions_);
00345
00346 if( outputExpressions_.size() > 0 ) {
00347 setupOutput( outputGrids_, outputExpressions_ );
00348 }
00349 else {
00350 std::vector<std::string> outputNames;
00351 std::vector<std::string> diffs_outputNames;
00352 std::vector<uint> dim_outputs;
00353 std::vector<DMatrix> outputDependencies_ = data.getOutputDependencies();
00354 data.getNameOutputs(outputNames);
00355 data.getNameDiffsOutputs(diffs_outputNames);
00356 data.getDimOutputs(dim_outputs);
00357 if( !data.hasCompressedStorage() ) {
00358 setupOutput( outputGrids_, outputNames, diffs_outputNames, dim_outputs );
00359 }
00360 else {
00361 setupOutput( outputGrids_, outputNames, diffs_outputNames, dim_outputs, outputDependencies_ );
00362 }
00363 }
00364 }
00365
00366 setup( );
00367
00368 return SUCCESSFUL_RETURN;
00369 }
00370
00371
00372 returnValue IntegratorExport::updateInputSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& tmp_index )
00373 {
00374 if( NX1 > 0 ) {
00375 ExportForLoop loop01( index1,0,NX1 );
00376 ExportForLoop loop02( index2,0,NX1 );
00377 loop02.addStatement( tmp_index == index2+index1*NX );
00378 loop02.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) == rk_diffsNew1.getSubMatrix( index1,index1+1,index2,index2+1 ) );
00379 loop01.addStatement( loop02 );
00380
00381 if( NU > 0 ) {
00382 ExportForLoop loop03( index2,0,NU );
00383 loop03.addStatement( tmp_index == index2+index1*NU );
00384 loop03.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) == rk_diffsNew1.getSubMatrix( index1,index1+1,NX1+index2,NX1+index2+1 ) );
00385 loop01.addStatement( loop03 );
00386 }
00387 block->addStatement( loop01 );
00388 }
00389
00390 return SUCCESSFUL_RETURN;
00391 }
00392
00393
00394 returnValue IntegratorExport::propagateInputSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2,
00395 const ExportIndex& index3, const ExportIndex& tmp_index )
00396 {
00397 if( NX1 > 0 ) {
00398 ExportForLoop loop01( index1,0,NX1 );
00399 ExportForLoop loop02( index2,0,NX1 );
00400 loop02.addStatement( tmp_index == index2+index1*NX );
00401 loop02.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) == rk_diffsNew1.getSubMatrix( index1,index1+1,0,1 )*rk_diffsPrev1.getSubMatrix( 0,1,index2,index2+1 ) );
00402 ExportForLoop loop03( index3,1,NX1 );
00403 loop03.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) += rk_diffsNew1.getSubMatrix( index1,index1+1,index3,index3+1 )*rk_diffsPrev1.getSubMatrix( index3,index3+1,index2,index2+1 ) );
00404 loop02.addStatement( loop03 );
00405 loop01.addStatement( loop02 );
00406
00407 if( NU > 0 ) {
00408 ExportForLoop loop04( index2,0,NU );
00409 loop04.addStatement( tmp_index == index2+index1*NU );
00410 loop04.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) == rk_diffsNew1.getSubMatrix( index1,index1+1,NX1+index2,NX1+index2+1 ) );
00411 ExportForLoop loop05( index3,0,NX1 );
00412 loop05.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) += rk_diffsNew1.getSubMatrix( index1,index1+1,index3,index3+1 )*rk_diffsPrev1.getSubMatrix( index3,index3+1,NX1+index2,NX1+index2+1 ) );
00413 loop04.addStatement( loop05 );
00414 loop01.addStatement( loop04 );
00415 }
00416 block->addStatement( loop01 );
00417 }
00418 return SUCCESSFUL_RETURN;
00419 }
00420
00421
00422 returnValue IntegratorExport::updateImplicitSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& tmp_index )
00423 {
00424 if( NX2 > 0 ) {
00425 ExportForLoop loop01( index1,NX1,NX1+NX2 );
00426 ExportForLoop loop02( index2,0,NX1+NX2 );
00427 loop02.addStatement( tmp_index == index2+index1*NX );
00428 loop02.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) == rk_diffsNew2.getSubMatrix( index1-NX1,index1-NX1+1,index2,index2+1 ) );
00429 loop01.addStatement( loop02 );
00430
00431 if( NU > 0 ) {
00432 ExportForLoop loop03( index2,0,NU );
00433 loop03.addStatement( tmp_index == index2+index1*NU );
00434 loop03.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) == rk_diffsNew2.getSubMatrix( index1-NX1,index1-NX1+1,NX1+NX2+index2,NX1+NX2+index2+1 ) );
00435 loop01.addStatement( loop03 );
00436 }
00437 block->addStatement( loop01 );
00438 }
00439
00440 if( NXA > 0 ) {
00441 ExportForLoop loop01( index1,NX,NX+NXA );
00442 ExportForLoop loop02( index2,0,NX1+NX2 );
00443 loop02.addStatement( tmp_index == index2+index1*NX );
00444 loop02.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) == rk_diffsNew2.getSubMatrix( index1-NX1-NX3,index1-NX1-NX3+1,index2,index2+1 ) );
00445 loop01.addStatement( loop02 );
00446
00447 if( NU > 0 ) {
00448 ExportForLoop loop03( index2,0,NU );
00449 loop03.addStatement( tmp_index == index2+index1*NU );
00450 loop03.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) == rk_diffsNew2.getSubMatrix( index1-NX1-NX3,index1-NX1-NX3+1,NX1+NX2+index2,NX1+NX2+index2+1 ) );
00451 loop01.addStatement( loop03 );
00452 }
00453 block->addStatement( loop01 );
00454 }
00455
00456 return SUCCESSFUL_RETURN;
00457 }
00458
00459
00460 returnValue IntegratorExport::propagateImplicitSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2,
00461 const ExportIndex& index3, const ExportIndex& tmp_index )
00462 {
00463 if( NX2 > 0 ) {
00464 ExportForLoop loop01( index1,NX1,NX1+NX2 );
00465 ExportForLoop loop02( index2,0,NX1 );
00466 loop02.addStatement( tmp_index == index2+index1*NX );
00467 loop02.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) == 0.0 );
00468 ExportForLoop loop03( index3,0,NX1 );
00469 loop03.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) += rk_diffsNew2.getSubMatrix( index1-NX1,index1-NX1+1,index3,index3+1 )*rk_diffsPrev1.getSubMatrix( index3,index3+1,index2,index2+1 ) );
00470 loop02.addStatement( loop03 );
00471 ExportForLoop loop04( index3,0,NX2 );
00472 loop04.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) += rk_diffsNew2.getSubMatrix( index1-NX1,index1-NX1+1,NX1+index3,NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( index3,index3+1,index2,index2+1 ) );
00473 loop02.addStatement( loop04 );
00474 loop01.addStatement( loop02 );
00475
00476 ExportForLoop loop05( index2,NX1,NX1+NX2 );
00477 loop05.addStatement( tmp_index == index2+index1*NX );
00478 loop05.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) == 0.0 );
00479 ExportForLoop loop06( index3,0,NX2 );
00480 loop06.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) += rk_diffsNew2.getSubMatrix( index1-NX1,index1-NX1+1,NX1+index3,NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( index3,index3+1,index2,index2+1 ) );
00481 loop05.addStatement( loop06 );
00482 loop01.addStatement( loop05 );
00483
00484 if( NU > 0 ) {
00485 ExportForLoop loop07( index2,0,NU );
00486 loop07.addStatement( tmp_index == index2+index1*NU );
00487 loop07.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) == rk_diffsNew2.getSubMatrix( index1-NX1,index1-NX1+1,NX1+NX2+index2,NX1+NX2+index2+1 ) );
00488 ExportForLoop loop08( index3,0,NX1 );
00489 loop08.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) += rk_diffsNew2.getSubMatrix( index1-NX1,index1-NX1+1,index3,index3+1 )*rk_diffsPrev1.getSubMatrix( index3,index3+1,NX1+index2,NX1+index2+1 ) );
00490 loop07.addStatement( loop08 );
00491 ExportForLoop loop09( index3,0,NX2 );
00492 loop09.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) += rk_diffsNew2.getSubMatrix( index1-NX1,index1-NX1+1,NX1+index3,NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( index3,index3+1,NX1+NX2+index2,NX1+NX2+index2+1 ) );
00493 loop07.addStatement( loop09 );
00494 loop01.addStatement( loop07 );
00495 }
00496 block->addStatement( loop01 );
00497 }
00498
00499
00500 return SUCCESSFUL_RETURN;
00501 }
00502
00503
00504 returnValue IntegratorExport::updateOutputSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& tmp_index )
00505 {
00506 if( NX3 > 0 ) {
00507 ExportForLoop loop01( index1,NX1+NX2,NX );
00508 ExportForLoop loop02( index2,0,NX );
00509 loop02.addStatement( tmp_index == index2+index1*NX );
00510 loop02.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) == rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,index2,index2+1 ) );
00511 loop01.addStatement( loop02 );
00512
00513 if( NU > 0 ) {
00514 ExportForLoop loop03( index2,0,NU );
00515 loop03.addStatement( tmp_index == index2+index1*NU );
00516 loop03.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) == rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,NX+index2,NX+index2+1 ) );
00517 loop01.addStatement( loop03 );
00518 }
00519 block->addStatement( loop01 );
00520 }
00521
00522 return SUCCESSFUL_RETURN;
00523 }
00524
00525
00526 returnValue IntegratorExport::propagateOutputSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2,
00527 const ExportIndex& index3, const ExportIndex& tmp_index )
00528 {
00529 if( NX3 > 0 ) {
00530 ExportForLoop loop01( index1,NX1+NX2,NX );
00531 ExportForLoop loop02( index2,0,NX1 );
00532 loop02.addStatement( tmp_index == index2+index1*NX );
00533 loop02.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) == 0.0 );
00534 ExportForLoop loop03( index3,0,NX1 );
00535 loop03.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) += rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,index3,index3+1 )*rk_diffsPrev1.getSubMatrix( index3,index3+1,index2,index2+1 ) );
00536 loop02.addStatement( loop03 );
00537 ExportForLoop loop04( index3,0,NX2 );
00538 loop04.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) += rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,NX1+index3,NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( index3,index3+1,index2,index2+1 ) );
00539 loop02.addStatement( loop04 );
00540 ExportForLoop loop042( index3,0,NX3 );
00541 loop042.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) += rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,NX1+NX2+index3,NX1+NX2+index3+1 )*rk_diffsPrev3.getSubMatrix( index3,index3+1,index2,index2+1 ) );
00542 loop02.addStatement( loop042 );
00543 loop01.addStatement( loop02 );
00544
00545 ExportForLoop loop05( index2,NX1,NX1+NX2 );
00546 loop05.addStatement( tmp_index == index2+index1*NX );
00547 loop05.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) == 0.0 );
00548 ExportForLoop loop06( index3,0,NX2 );
00549 loop06.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) += rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,NX1+index3,NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( index3,index3+1,index2,index2+1 ) );
00550 loop05.addStatement( loop06 );
00551 ExportForLoop loop062( index3,0,NX3 );
00552 loop062.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) += rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,NX1+NX2+index3,NX1+NX2+index3+1 )*rk_diffsPrev3.getSubMatrix( index3,index3+1,index2,index2+1 ) );
00553 loop05.addStatement( loop062 );
00554 loop01.addStatement( loop05 );
00555
00556 ExportForLoop loop07( index2,NX1+NX2,NX );
00557 loop07.addStatement( tmp_index == index2+index1*NX );
00558 loop07.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) == 0.0 );
00559 ExportForLoop loop08( index3,0,NX3 );
00560 loop08.addStatement( rk_eta.getCol( tmp_index+NX+NXA ) += rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,NX1+NX2+index3,NX1+NX2+index3+1 )*rk_diffsPrev3.getSubMatrix( index3,index3+1,index2,index2+1 ) );
00561 loop07.addStatement( loop08 );
00562 loop01.addStatement( loop07 );
00563
00564 if( NU > 0 ) {
00565 ExportForLoop loop09( index2,0,NU );
00566 loop09.addStatement( tmp_index == index2+index1*NU );
00567 loop09.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) == rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,NX+index2,NX+index2+1 ) );
00568 ExportForLoop loop10( index3,0,NX1 );
00569 loop10.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) += rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,index3,index3+1 )*rk_diffsPrev1.getSubMatrix( index3,index3+1,NX1+index2,NX1+index2+1 ) );
00570 loop09.addStatement( loop10 );
00571 ExportForLoop loop11( index3,0,NX2 );
00572 loop11.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) += rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,NX1+index3,NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( index3,index3+1,NX1+NX2+index2,NX1+NX2+index2+1 ) );
00573 loop09.addStatement( loop11 );
00574 ExportForLoop loop112( index3,0,NX3 );
00575 loop112.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX) ) += rk_diffsNew3.getSubMatrix( index1-NX1-NX2,index1-NX1-NX2+1,NX1+NX2+index3,NX1+NX2+index3+1 )*rk_diffsPrev3.getSubMatrix( index3,index3+1,NX+index2,NX+index2+1 ) );
00576 loop09.addStatement( loop112 );
00577 loop01.addStatement( loop09 );
00578 }
00579 block->addStatement( loop01 );
00580 }
00581
00582 return SUCCESSFUL_RETURN;
00583 }
00584
00585
00586
00587
00588
00589
00590 DMatrix IntegratorExport::expandOutputMatrix( const DMatrix& A3 ) {
00591 DMatrix result = zeros<double>(NX3,NX);
00592 uint i,j;
00593 for( i = 0; i < NX3; i++ ) {
00594 for( j = 0; j < NX3; j++ ) {
00595 result(i,NX-NX3+j) = A3(i,j);
00596 }
00597 }
00598
00599 return result;
00600 }
00601
00602
00603 returnValue IntegratorExport::copy( const IntegratorExport& arg
00604 )
00605 {
00606 exportRhs = arg.exportRhs;
00607 crsFormat = arg.crsFormat;
00608 grid = arg.grid;
00609 numSteps = arg.numSteps;
00610
00611
00612 integrate = arg.integrate;
00613
00614 return SUCCESSFUL_RETURN;
00615 }
00616
00617
00618 returnValue IntegratorExport::clear( )
00619 {
00620 return SUCCESSFUL_RETURN;
00621 }
00622
00623
00624 uint IntegratorExport::getIntegrationInterval( double time ) {
00625 uint index = 0;
00626 double scale = 1.0/(grid.getLastTime() - grid.getFirstTime());
00627 while( index < (grid.getNumIntervals()-1) && time > scale*grid.getTime( index+1 ) ) {
00628 index++;
00629 }
00630 return index;
00631 }
00632
00633
00634 returnValue IntegratorExport::getGrid( Grid& grid_ ) const{
00635
00636 grid_ = grid;
00637 return SUCCESSFUL_RETURN;
00638 }
00639
00640
00641 returnValue IntegratorExport::getNumSteps( DVector& _numSteps ) const{
00642
00643 _numSteps = numSteps;
00644 return SUCCESSFUL_RETURN;
00645 }
00646
00647
00648 returnValue IntegratorExport::getOutputExpressions( std::vector<Expression>& outputExpressions_ ) const{
00649
00650 outputExpressions_ = outputExpressions;
00651 return SUCCESSFUL_RETURN;
00652 }
00653
00654
00655 returnValue IntegratorExport::getOutputGrids( std::vector<Grid>& outputGrids_ ) const{
00656
00657 outputGrids_ = outputGrids;
00658 return SUCCESSFUL_RETURN;
00659 }
00660
00661 bool IntegratorExport::equidistantControlGrid( ) const{
00662
00663 return numSteps.isEmpty();
00664 }
00665
00666 const std::string IntegratorExport::getNameRHS() const{
00667 return rhs.getName();
00668 }
00669
00670 const std::string IntegratorExport::getNameFullRHS() const{
00671 if( NX2 == NX ) {
00672 return getNameRHS();
00673 }
00674 else {
00675 return fullRhs.getName();
00676 }
00677 }
00678
00679 const std::string IntegratorExport::getNameOutputRHS() const{
00680 return rhs3.getName();
00681 }
00682
00683 const std::string IntegratorExport::getNameOutputDiffs() const{
00684 return diffs_rhs3.getName();
00685 }
00686
00687 const std::string IntegratorExport::getNameOUTPUT( uint index ) const{
00688 return outputs[index].getName();
00689 }
00690
00691 uint IntegratorExport::getDimOUTPUT( uint index ) const{
00692 if( exportRhs ) {
00693 return outputExpressions[index].getDim();
00694 }
00695 else {
00696 return num_outputs[index];
00697 }
00698 }
00699
00700
00701 const std::string IntegratorExport::getNameDiffsRHS() const{
00702 return diffs_rhs.getName();
00703 }
00704
00705 const std::string IntegratorExport::getNameDiffsOUTPUT( uint index ) const{
00706 return diffs_outputs[index].getName();
00707 }
00708
00709
00710 CLOSE_NAMESPACE_ACADO
00711
00712