ACADOintegrators.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2009 by Boris Houska and Hans Joachim Ferreau, K.U.Leuven.
00006  *    Developed within the Optimization in Engineering Center (OPTEC) under
00007  *    supervision of Moritz Diehl. All rights reserved.
00008  *
00009  *    ACADO Toolkit is free software; you can redistribute it and/or
00010  *    modify it under the terms of the GNU Lesser General Public
00011  *    License as published by the Free Software Foundation; either
00012  *    version 3 of the License, or (at your option) any later version.
00013  *
00014  *    ACADO Toolkit is distributed in the hope that it will be useful,
00015  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00017  *    Lesser General Public License for more details.
00018  *
00019  *    You should have received a copy of the GNU Lesser General Public
00020  *    License along with ACADO Toolkit; if not, write to the Free Software
00021  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00022  *
00023  */
00024 
00025 
00032 #include <acado_integrators.hpp>
00033 #include <acado/utils/matlab_acado_utils.hpp>
00034 
00035 
00036 USING_NAMESPACE_ACADO
00037 
00038 
00039 #include "model_include.hpp"
00040 
00041 //#include "stdafx.h"
00042 
00043 
00044 // global pointer to Matlab function handle,
00045 // if dynamic model is given like that
00046 mxArray* ModelFcn_f = NULL;
00047 DifferentialEquation* modelFcn = NULL;
00048 mxArray* ModelFcn_jac = NULL;
00049 
00050 mxArray* ModelFcnT  = NULL;
00051 mxArray* ModelFcnX  = NULL;
00052 mxArray* ModelFcnXA = NULL;
00053 mxArray* ModelFcnU  = NULL;
00054 mxArray* ModelFcnP  = NULL;
00055 mxArray* ModelFcnW  = NULL;
00056 mxArray* ModelFcnDX = NULL;
00057 
00058 unsigned int modelFcnNX  = 0;
00059 unsigned int modelFcnNXA = 0;
00060 unsigned int modelFcnNU  = 0;
00061 unsigned int modelFcnNP  = 0;
00062 unsigned int modelFcnNW  = 0;
00063 unsigned int modelFcnNDX = 0;
00064 
00065 unsigned int jacobianNumber = -1;
00066 double* f_store                         = NULL;
00067 double* J_store                         = NULL;
00068 
00069 void clearAllGlobals( )
00070 {
00071         // first, clear all static counters within the toolkit
00072         clearAllStaticCounters( );
00073 
00074         // second, clear all global variables of the interface
00075         if ( modelFcn != NULL )
00076         {
00077                 modelFcn = NULL;
00078                 delete modelFcn;
00079         }
00080 
00081         if ( f_store != NULL )
00082         {
00083                 f_store = NULL;
00084                 delete f_store;
00085         }
00086 
00087         if ( J_store != NULL )
00088         {
00089                 J_store = NULL;
00090                 delete J_store;
00091         }
00092 
00093         if ( ModelFcn_f != NULL )
00094         {
00095                 mxDestroyArray( ModelFcn_f );
00096                 ModelFcn_f = NULL;
00097         }
00098 
00099         if ( ModelFcnT != NULL )
00100         {
00101                 mxDestroyArray( ModelFcnT );
00102                 ModelFcnT = NULL;
00103         }
00104 
00105         if ( ModelFcnX != NULL )
00106         {
00107                 mxDestroyArray( ModelFcnX );
00108                 ModelFcnX = NULL;
00109         }
00110 
00111         if ( ModelFcnXA != NULL )
00112         {
00113                 mxDestroyArray( ModelFcnXA );
00114                 ModelFcnXA = NULL;
00115         }
00116 
00117         if ( ModelFcnU != NULL )
00118         {
00119                 mxDestroyArray( ModelFcnU );
00120                 ModelFcnU = NULL;
00121         }
00122 
00123         if ( ModelFcnP != NULL )
00124         {
00125                 mxDestroyArray( ModelFcnP );
00126                 ModelFcnP = NULL;
00127         }
00128 
00129         if ( ModelFcnW != NULL )
00130         {
00131                 mxDestroyArray( ModelFcnW );
00132                 ModelFcnW = NULL;
00133         }
00134 
00135         if ( ModelFcnDX != NULL )
00136         {
00137                 mxDestroyArray( ModelFcnDX );
00138                 ModelFcnDX = NULL;
00139         }
00140 
00141         if ( ModelFcn_jac != NULL )
00142         {
00143                 mxDestroyArray( ModelFcn_jac );
00144                 ModelFcn_jac = NULL;
00145         }
00146 
00147 
00148         modelFcnNX  = 0;
00149         modelFcnNXA = 0;
00150         modelFcnNU  = 0;
00151         modelFcnNP  = 0;
00152         modelFcnNW  = 0;
00153         modelFcnNDX = 0;
00154         jacobianNumber = -1;
00155 }
00156 
00157 
00158 // generic dynamic model function for evaluating
00159 // Matlab function handles from C
00160 void genericODE( double* x, double* f, void *userData  )
00161 {
00162         unsigned int i;
00163 
00164         double* tt = mxGetPr( ModelFcnT );
00165         tt[0] = x[0];
00166 
00167         double* xx = mxGetPr( ModelFcnX );
00168         for( i=0; i<modelFcnNX; ++i )
00169                 xx[i] = x[i+1];
00170 
00171         double* uu = mxGetPr( ModelFcnU );
00172         for( i=0; i<modelFcnNU; ++i )
00173                 uu[i] = x[i+1+modelFcnNX];
00174 
00175         double* pp = mxGetPr( ModelFcnP );
00176         for( i=0; i<modelFcnNP; ++i )
00177                 pp[i] = x[i+1+modelFcnNX+modelFcnNU];
00178 
00179         double* ww = mxGetPr( ModelFcnW );
00180         for( i=0; i<modelFcnNW; ++i )
00181                 ww[i] = x[i+1+modelFcnNX+modelFcnNU+modelFcnNP];
00182 
00183         mxArray* FF = NULL;
00184 
00185         mxArray* argIn[]  = { ModelFcn_f,ModelFcnT,ModelFcnX,ModelFcnU,ModelFcnP,ModelFcnW };
00186         mxArray* argOut[] = { FF };
00187 
00188         mexCallMATLAB( 1,argOut, 6,argIn,"generic_ode" );
00189 
00190 
00191         double* ff = mxGetPr( *argOut );
00192 
00193         for( i=0; i<modelFcnNX; ++i )
00194                 f[i] = ff[i];
00195 
00196         mxDestroyArray( *argOut );
00197 
00198 }
00199 
00200 
00201 void genericJacobian ( int number, double* x, double* seed, double* f, double* df, void *userData )
00202 {
00203         unsigned int i, j;
00204         double* ff;
00205         double* J;
00206 
00207 
00208         if (J_store == NULL){
00209 
00210                 J_store = (double*) calloc ((modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW)*(modelFcnNX+modelFcnNXA),sizeof(double));
00211                 f_store = (double*) calloc (modelFcnNX,sizeof(double));
00212 
00213         }
00214 
00215 
00216         if ( (int) jacobianNumber == number){
00217 
00218                 //mexPrintf("\n SAME JACOBIAN --- Number:%d", number);
00219                 //ff = f_store;
00220                 J = J_store;
00221                 ff = f_store;
00222 
00223                 for( i=0; i<modelFcnNX+modelFcnNXA; ++i ) {
00224                         df[i] = 0;
00225                         f[i] = 0; //F[i]
00226                         for (j=0; j < modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW; ++j){
00227                                 df[i] += J[(j*(modelFcnNX+modelFcnNXA))+i]*seed[j+1];  //  J+1 since we do not use nt here
00228                                 //mexPrintf ("\n jac %d, %d = %f -- seed: %f -- function : %f", i, j, J[(j*(modelFcnNX+modelFcnNXA))+i], seed[j+1], f[i]);
00229                         }
00230                 }
00231 
00232 
00233                 for( i=0; i<modelFcnNX; ++i ){
00234                         f[i] = ff[i];
00235                         //mexPrintf("\n function EVAL %d --> %f", i, f[i]);
00236                 }
00237 
00238 
00239         }else{
00240                 //mexPrintf("\n DIFFERENT JACOBIAN --- Number:%d", number);
00241 
00242                 jacobianNumber = number;   // Store current number in global.
00243 
00244 
00245                 // Set the values to pass to generic_jacobian
00246                 double* tt = mxGetPr( ModelFcnT );
00247                 tt[0] = x[0];
00248 
00249                 double* xx = mxGetPr( ModelFcnX );
00250                 for( i=0; i<modelFcnNX; ++i )
00251                         xx[i] = x[i+1];
00252 
00253                 double* uu = mxGetPr( ModelFcnU );
00254                 for( i=0; i<modelFcnNU; ++i )
00255                         uu[i] = x[i+1+modelFcnNX];
00256 
00257                 double* pp = mxGetPr( ModelFcnP );
00258                 for( i=0; i<modelFcnNP; ++i )
00259                         pp[i] = x[i+1+modelFcnNX+modelFcnNU];
00260 
00261                 double* ww = mxGetPr( ModelFcnW );
00262                 for( i=0; i<modelFcnNW; ++i )
00263                         ww[i] = x[i+1+modelFcnNX+modelFcnNU+modelFcnNP];
00264 
00265                 // Execute generic_jacobian
00266                 mxArray* FF = NULL;
00267                 mxArray* argIn[]  = { ModelFcn_jac,ModelFcnT,ModelFcnX,ModelFcnU,ModelFcnP,ModelFcnW };
00268                 mxArray* argOut[] = { FF };
00269 
00270                 //mexPrintf("\n EVAL JACOBIAN - JAC");
00271                 mexCallMATLAB( 1,argOut, 6,argIn,"generic_jacobian" );
00272 
00273                 // Examine output
00274                 unsigned int rowLen = mxGetM(*argOut);
00275                 unsigned int colLen = mxGetN(*argOut);
00276 
00277 
00278                 if (rowLen != modelFcnNX+modelFcnNXA)
00279                 {
00280                         mexErrMsgTxt( "ERROR (ACADOintegrator): Jacobian matrix rows do not match (should be modelFcnNX+modelFcnNXA). " );
00281                 }
00282 
00283                 if (colLen != modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW)
00284                 {
00285                         mexErrMsgTxt( "ERROR (ACADOintegrator): Jacobian matrix columns do not match (should be modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW). " );
00286                 }
00287 
00288                 J = mxGetPr( *argOut );
00289 
00290                 memcpy(J_store, J, (modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW)*(modelFcnNX+modelFcnNXA) * sizeof ( double ));
00291 
00292 
00293 
00294                 for( i=0; i<modelFcnNX+modelFcnNXA; ++i ) {
00295                         df[i] = 0;
00296                         f[i] = 0; //F[i]
00297                         for (j=0; j < modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW; ++j){
00298                                 df[i] += J[(j*(modelFcnNX+modelFcnNXA))+i]*seed[j+1];  //  J+1 since we do not use nt here
00299                                 //mexPrintf ("\n jac %d, %d = %f -- seed: %f -- function : %f", i, j, J[(j*(modelFcnNX+modelFcnNXA))+i], seed[j+1], f[i]);
00300                         }
00301                 }
00302 
00303 
00304 
00305 
00306                 mxArray* FF2 = NULL;
00307                 mxArray* argIn2[]  = { ModelFcn_f,ModelFcnT,ModelFcnX,ModelFcnU,ModelFcnP,ModelFcnW };
00308                 mxArray* argOut2[] = { FF2 };
00309 
00310                 mexCallMATLAB( 1,argOut2, 6,argIn2,"generic_ode" );
00311 
00312 
00313                 ff = mxGetPr( *argOut2 );
00314                 memcpy(f_store, ff, (modelFcnNX) * sizeof ( double ));
00315 
00316 
00317                 for( i=0; i<modelFcnNX; ++i ){
00318                         f[i] = ff[i];
00319                         //mexPrintf("\n function EVAL %d --> %f", i, f[i]);
00320                 }
00321 
00322                 mxDestroyArray( *argOut );
00323                 mxDestroyArray( *argOut2 );
00324 
00325 
00326         }
00327 
00328 
00329 
00330         //mexPrintf("\n\n");
00331 }
00332 
00333 // generic dynamic model function for evaluating
00334 // Matlab function handles from C
00335 void genericDAE( double* x, double* f, void *userData )
00336 {
00337         unsigned int i;
00338 
00339         double* tt = mxGetPr( ModelFcnT );
00340         tt[0] = x[0];
00341 
00342         double* xx = mxGetPr( ModelFcnX );
00343         for( i=0; i<modelFcnNX; ++i )
00344                 xx[i] = x[i+1];
00345 
00346         double* xxa = mxGetPr( ModelFcnXA );
00347         for( i=0; i<modelFcnNXA; ++i )
00348                 xxa[i] = x[i+1+modelFcnNX];
00349 
00350         double* uu = mxGetPr( ModelFcnU );
00351         for( i=0; i<modelFcnNU; ++i )
00352                 uu[i] = x[i+1+modelFcnNX+modelFcnNXA];
00353 
00354         double* pp = mxGetPr( ModelFcnP );
00355         for( i=0; i<modelFcnNP; ++i )
00356                 pp[i] = x[i+1+modelFcnNX+modelFcnNXA+modelFcnNU];
00357 
00358         double* ww = mxGetPr( ModelFcnW );
00359         for( i=0; i<modelFcnNW; ++i )
00360                 ww[i] = x[i+1+modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP];
00361     
00362         // call matlab model via function handle and stack results
00363         mxArray* argIn_f[]  = { ModelFcn_f,ModelFcnT,ModelFcnX,ModelFcnXA,ModelFcnU,ModelFcnP,ModelFcnW }; 
00364 
00365         mxArray* FF = NULL;
00366         double*  ff = NULL;
00367         mxArray* argOut[] = { FF };
00368     
00370 
00371     mexCallMATLAB( 1,argOut, 7,argIn_f,"generic_dae" );
00372         ff = mxGetPr( *argOut );
00373         for( i=0; i<modelFcnNX+modelFcnNXA; ++i )
00374                 f[i] = ff[i];
00375 
00376     //printf("after DAE call\n");
00377     
00378         mxDestroyArray( *argOut );
00379 }
00380 
00381 
00382 unsigned int determineNumberOfAlgebraicStates( )
00383 {
00384         return 0;
00385 }
00386 
00387 
00388 void plotErrorMessage(  returnValue returnvalue,
00389                                                 double* PrintLevel_
00390                                                 )
00391 {
00392         if ( ( PrintLevel_ != NULL ) && ( PrintLevel_[0] <= 0.0 ) )
00393         {
00394                 switch ( returnvalue )
00395                 {
00396                         case RET_MISSING_INPUTS:
00397                                 mexPrintf( "ERROR (ACADOintegrator): The integrator misses some inputs.\n" );
00398         
00399                         case RET_TO_SMALL_OR_NEGATIVE_TIME_INTERVAL:
00400                                 mexPrintf( "ERROR (ACADOintegrator): The integration interval is too small or negative.\n" );
00401         
00402                         case RET_MAX_NUMBER_OF_STEPS_EXCEEDED:
00403                                 mexPrintf( "ERROR (ACADOintegrator): The maximum number of integration steps is exceeded.\n" );
00404         
00405                         case RET_INPUT_HAS_WRONG_DIMENSION:
00406                                 mexPrintf( "ERROR (ACADOintegrator): At least one of the inputs has a wrong dimension.\n" );
00407         
00408                         case SUCCESSFUL_RETURN:
00409                                 return;
00410         
00411                         default:
00412                                 mexPrintf( "ERROR (ACADOintegrator): Unsuccessful return from the integrator.\n");
00413                 }
00414         }
00415 }
00416 
00420 DifferentialEquation* allocateDifferentialEquation(     char *modelName,                // name of the model
00421                                                                                                         char *integratorName    // name of the integrator
00422                                                                                                         )
00423 {
00424         DifferentialEquation* f = NULL;
00425 
00426         if( strcmp(integratorName,"DiscretizedODE") == 0 )
00427                 f = new DiscretizedDifferentialEquation;
00428         else
00429                 f = new DifferentialEquation;
00430 
00431         if ( modelName != NULL )
00432         {
00433                 // function name is given as string
00434                 void (*fcn)( DifferentialEquation* );
00435                 fcn = allocateFunctionPointer( modelName );
00436 
00437                 if( fcn == 0 )
00438                 {
00439                         delete f;
00440                         clearAllGlobals( );
00441                         mexErrMsgTxt( "ERROR (ACADOintegrator): A model with the specified name does not exist.\n" );
00442                 }
00443 
00444                 fcn( f );
00445         }
00446 
00447         return f;
00448 }
00449 
00450 
00451 //allocateJacobian
00452 
00453 
00457 Integrator* allocateIntegrator( char *integratorName,           // name of the integrator
00458                                                                 DifferentialEquation* f         // allocated differential equation
00459                                                                 )
00460 {
00461         Integrator* integrator = NULL;
00462 
00463         if ( f == NULL )
00464                 mexErrMsgTxt( "bug!" );
00465 
00466         if( strcmp(integratorName,"RK12") == 0 )
00467                 integrator = new IntegratorRK12( *f );
00468 
00469         if( strcmp(integratorName,"RK23") == 0 )
00470                 integrator = new IntegratorRK23( *f );
00471 
00472         if( strcmp(integratorName,"RK45") == 0 )
00473                 integrator = new IntegratorRK45( *f );
00474 
00475         if( strcmp(integratorName,"RK78") == 0 )
00476                 integrator = new IntegratorRK78( *f );
00477 
00478         if( strcmp(integratorName,"DiscretizedODE") == 0 )
00479                 integrator = new IntegratorDiscretizedODE( *f );
00480 
00481         if( strcmp(integratorName,"BDF") == 0 )
00482                 integrator = new IntegratorBDF( *f );
00483 
00484         return integrator;
00485 }
00486 
00487 
00488 // --------------------------------------------------------------------------
00489 
00490               // ********************************//
00491               //                                 //
00492               //      m e x F u n c t i o n      //
00493               // ________________________________//
00494 
00513 void mexFunction(       int nlhs,       mxArray *plhs[],
00514                                         int nrhs, const mxArray *prhs[]
00515                                         )
00516 {
00517         unsigned int i,j;
00518 
00519 
00520         // INPUT ARGUMENTS:
00521         // ---------------------------------------------------------------
00522 
00523         // integrator settings struct
00524         const mxArray *IntegratorSettings = NULL;
00525 
00526         // start point for differential states
00527         double *xStart = NULL;
00528 
00529         // initial value for algebraic states (only for DAEs)
00530         double *xaStart = NULL;
00531 
00532         // start of the integration interval
00533         double *tStart = NULL;
00534         int tStartIdx = -1;
00535 
00536         // end of the integration interval
00537         double *tEnd = NULL;
00538         int tEndIdx = -1;
00539 
00540         // is dynamic model an ODE (DAE otherwise)?
00541         BooleanType isODE = BT_TRUE;
00542 
00543         // end value of algebraic states is to be returned to plhs[xaEndIdx] (= -1 for none)
00544         int xaEndIdx = -1;
00545 
00546         // an output struct is to be returned to plhs[outputIdx] (= -1 for none)
00547         int outputIdx = -1;
00548 
00549 
00550         // SETTINGS:
00551         // ---------------------------------------------------------------
00552 
00553         // name of the model
00554         mxArray   *ModelName = NULL;
00555         char      *modelName = NULL;  // (if specified as string, otherwise global modelFcn is used)
00556 
00557         // name of the integrator
00558         mxArray   *IntegratorName = NULL;
00559         char      *integratorName = NULL;
00560 
00561         // tolerance of the integrator
00562         mxArray   *Tol = NULL;
00563         double    *tol = NULL;
00564 
00565         // tolerance of the integrator
00566         mxArray   *aTol = NULL;
00567         double    *atol = NULL;
00568 
00569         // maximum number of allowed steps
00570         mxArray   *MaxNumStep = NULL;
00571         double    *maxNumStep = NULL;
00572 
00573         // min step size
00574         mxArray   *MinStep = NULL;
00575         double    *minStep = NULL;
00576     
00577     // initial step size
00578         mxArray   *InitialStep = NULL;
00579         double    *initialStep = NULL;
00580     
00581         // max step size
00582         mxArray   *MaxStep = NULL;
00583         double    *maxStep = NULL;
00584 
00585         // tuning
00586         mxArray   *StepTuning = NULL;
00587         double    *stepTuning = NULL;
00588 
00589         // linear algebra solver
00590         mxArray   *LinearAlgebraSolver = NULL;
00591         char      *linearAlgebraSolver = NULL;
00592 
00593         // controls
00594         mxArray   *U = NULL;
00595         double    *u = NULL;
00596 
00597         // parameters
00598         mxArray   *P = NULL;
00599         double    *p = NULL;
00600 
00601         // disturbances
00602         mxArray   *W = NULL;
00603         double    *w = NULL;
00604 
00605         // initial value for xdot (only for DAEs)
00606         mxArray   *DxInit = NULL;
00607         double    *dxInit = NULL;
00608 
00609         // sensitivity mode
00610         mxArray   *SensitivityMode = NULL;
00611         char      *sensitivityMode = NULL;
00612 
00613         // backward seed
00614         mxArray   *Bseed = NULL;
00615         double    *bseed = NULL;
00616 
00617         // forward seed (w.r.t. x)
00618         mxArray   *Dx = NULL;
00619         double    *dx = NULL;
00620 
00621         // forward seed (w.r.t. p)
00622         mxArray   *Dp = NULL;
00623         double    *dp = NULL;
00624 
00625         // forward seed (w.r.t. u)
00626         mxArray   *Du = NULL;
00627         double    *du = NULL;
00628 
00629         // forward seed (w.r.t. w)
00630         mxArray   *Dw = NULL;
00631         double    *dw = NULL;
00632 
00633         // backward seed (2nd order)
00634         mxArray   *Bseed2 = NULL;
00635         double    *bseed2 = NULL;
00636 
00637         // forward seed (w.r.t. w) (2nd order)
00638         mxArray   *Dx2 = NULL;
00639         double    *dx2 = NULL;
00640 
00641         // forward seed (w.r.t. w) (2nd order)
00642         mxArray   *Dp2 = NULL;
00643         double    *dp2 = NULL;
00644 
00645         // forward seed (w.r.t. w) (2nd order)
00646         mxArray   *Du2 = NULL;
00647         double    *du2 = NULL;
00648 
00649         // forward seed (w.r.t. w) (2nd order)
00650         mxArray   *Dw2 = NULL;
00651         double    *dw2 = NULL;
00652 
00653         // defines if (error) messages shall be printed or not
00654         mxArray   *PrintLevel_ = NULL;
00655         double    *printLevel_  = NULL;
00656 
00657         // array containing the indices of the states whose trajectory shall be plotted
00658         mxArray   *PlotXTrajectory = NULL;
00659         mxArray   *PlotXaTrajectory = NULL;
00660 
00661         // defines if subplots are to be used
00662         mxArray   *UseSubplots = NULL;
00663 
00664         // tolerance of the integrator
00665         mxArray   *Jacobian = NULL;
00666         double    *jacobian = NULL;
00667 
00668     // OUTPUTS:
00669     // -----------------------------------------------------------------
00670 
00671         // end values of differential and algebraic states
00672         double  *xEnd = NULL;
00673         double  *xaEnd = NULL;
00674 
00675         // integrator status
00676         mxArray *Status = NULL;
00677         double  *statusPtr = NULL;
00678 
00679         // number of integrator steps
00680         mxArray *NumberOfSteps = NULL;
00681         double  *numberOfSteps = NULL;
00682 
00683         // number of rejected integrator steps
00684         mxArray *NumberOfRejectedSteps = NULL;
00685         double  *numberOfRejectedSteps = NULL;
00686     
00687     // step size
00688         mxArray *GetStepSize = NULL;
00689         double  *getStepSize = NULL;
00690     
00691     // corrector tollerance
00692         mxArray *CorrectorTolerance = NULL;
00693         double  *correctorTolerance = NULL;
00694 
00695         // x trajectory values 
00696         mxArray *XTrajectory = NULL;
00697         double  *xTrajectory = NULL;
00698 
00699         // xa trajectory values 
00700         mxArray *XaTrajectory = NULL;
00701         double  *xaTrajectory = NULL;
00702 
00703         // sensitivity w.r.t everything (in forward mode only)
00704         mxArray *JJ = NULL;
00705         double  *jj = NULL;
00706 
00707         // sensitivity w.r.t the states (in backward mode only)
00708         mxArray *Jx = NULL;
00709         double  *jx = NULL;
00710 
00711         // sensitivity w.r.t the parameters (in backward mode only)
00712         mxArray *Jp = NULL;
00713         double  *jp = NULL;
00714 
00715         // sensitivity w.r.t the controls (in backward mode only)
00716         mxArray *Ju = NULL;
00717         double  *ju = NULL;
00718 
00719         // sensitivity w.r.t the disturbances (in backward mode only)
00720         mxArray *Jw = NULL;
00721         double  *jw = NULL;
00722 
00723         // second order sensitivity w.r.t everything (in forward mode 2 only)
00724         mxArray *JJ2 = NULL;
00725         double  *jj2 = NULL;
00726 
00727         // second order sensitivity w.r.t the states (in backward mode only)
00728         mxArray *Jx2 = NULL;
00729         double  *jx2 = NULL;
00730 
00731         // second order sensitivity w.r.t the parameters (in backward mode only)
00732         mxArray *Jp2 = NULL;
00733         double  *jp2 = NULL;
00734 
00735         // second order sensitivity w.r.t the controls (in backward mode only)
00736         mxArray *Ju2 = NULL;
00737         double  *ju2 = NULL;
00738 
00739         // second order sensitivity w.r.t the disturbances (in backward mode only)
00740         mxArray *Jw2 = NULL;
00741         double  *jw2 = NULL;
00742 
00743         // second order sensitivity w.r.t the disturbances (in backward mode only)
00744         mxArray *StorageResolution = NULL;
00745         double  *storageResolution = NULL;
00746 
00747 
00748         // DIMENSIONS:
00749         // ----------------------------  -------------------------------------
00750         unsigned int nx     = 0;  // number of differential states
00751         unsigned int nxa    = 0;  // number of algebraic states
00752         unsigned int nu     = 0;  // number of controls
00753         unsigned int np     = 0;  // number of parameters
00754         unsigned int nw     = 0;  // number of disturbances
00755         unsigned int nbDir  = 0;  // number of backward directions
00756         unsigned int nfDir  = 0;  // number of forward directions
00757         unsigned int nbDir2 = 0;  // number of 2nd order backward directions
00758         unsigned int nfDir2 = 0;  // number of 2nd order forward directions
00759 
00760 
00761         // I) CONSISTENCY CHECKS:
00762         // ----------------------
00763 
00764         if ( ( nrhs < 4 ) || ( nrhs > 5 ) )
00765                 mexErrMsgTxt( "ERROR (ACADOintegrator): Invalid number of input arguments!\n    Type 'help ACADOintegrators' for further information." );
00766 
00767         if ( ( nlhs < 1 ) || ( nlhs > 3 ) )
00768                 mexErrMsgTxt( "ERROR (ACADOintegrator): Invalid number of output arguments!\n    Type 'help ACADOintegrators' for further information." );
00769 
00770 
00771         // II. READING THE INPUT ARGUMENTS:
00772         // --------------------------------
00773 
00774         if( !mxIsEmpty( prhs[0]) )
00775         {
00776                 IntegratorSettings = prhs[0];
00777                 if ( !mxIsStruct( IntegratorSettings ) )
00778                         mexErrMsgTxt( "ERROR (ACADOintegrator): No integrator settings defined!" );
00779         }
00780         else
00781                 mexErrMsgTxt( "ERROR (ACADOintegrator): No integrator settings defined!" );
00782 
00783         if( !mxIsEmpty( prhs[1]) )
00784         {
00785                 xStart = mxGetPr( prhs[1] );
00786                 nx     = mxGetM( prhs[1] );
00787 
00788                 if ( mxGetN( prhs[1] ) != 1 )
00789                         mexErrMsgTxt( "ERROR (ACADOintegrator): Start value for differential state needs to be given as column vector." );
00790         }
00791         else
00792                 mexErrMsgTxt( "ERROR (ACADOintegrator): No initial value specified." );
00793 
00794         if ( nrhs == 4 )
00795         {
00796                 // ODE system
00797                 // An ODE system is supposed when ACADOintegrators() is called with 4 arguments
00798                 isODE = BT_TRUE;
00799                 tStartIdx = 2;
00800                 tEndIdx   = 3;
00801         }
00802         else
00803         {
00804                 // DAE system, xaStart optional
00805                 isODE = BT_FALSE;
00806                 tStartIdx = 3;
00807                 tEndIdx   = 4;
00808 
00809                 if( !mxIsEmpty( prhs[2]) )
00810                 {
00811                         xaStart = mxGetPr( prhs[2] );
00812                         nxa     = mxGetM( prhs[2] );
00813         
00814                         if ( mxGetN( prhs[2] ) != 1 )
00815                                 mexErrMsgTxt( "ERROR (ACADOintegrator): Start value for algebraic state needs to be given as column vector." );
00816                 }
00817         }
00818 
00819         if( !mxIsEmpty( prhs[tStartIdx]) )
00820         {
00821                 tStart = mxGetPr( prhs[tStartIdx] );
00822 
00823                 if ( isScalar( prhs[tStartIdx] ) == 0 )
00824                         mexErrMsgTxt( "ERROR (ACADOintegrator): Start time needs to be a scalar." );
00825         }
00826         else
00827                 mexErrMsgTxt( "ERROR (ACADOintegrator): No start time defined." );
00828 
00829 
00830         if( !mxIsEmpty( prhs[tEndIdx]) )
00831         {
00832                 tEnd = mxGetPr( prhs[tEndIdx] );
00833 
00834                 if ( isScalar( prhs[tEndIdx] ) == 0 )
00835                         mexErrMsgTxt( "ERROR (ACADOintegrator): End time needs to be a scalar." );
00836         }
00837         else
00838                 mexErrMsgTxt( "ERROR (ACADOintegrator): No end time defined." );
00839 
00840 
00841         // III. READING THE SETTINGS STRUCT:
00842         // ---------------------------------
00843 
00844         ModelName           = mxGetField( IntegratorSettings,0,"Model" );
00845         IntegratorName      = mxGetField( IntegratorSettings,0,"Integrator" );
00846         Tol                 = mxGetField( IntegratorSettings,0,"Tolerance" );
00847         aTol                = mxGetField( IntegratorSettings,0,"AbsoluteTolerance" );
00848         MaxNumStep          = mxGetField( IntegratorSettings,0,"MaxNumberOfSteps" );
00849         MinStep             = mxGetField( IntegratorSettings,0,"MinimumStepSize" );
00850     InitialStep         = mxGetField( IntegratorSettings,0,"InitialStepSize" );
00851         MaxStep                 = mxGetField( IntegratorSettings,0,"MaximumStepSize" );
00852     CorrectorTolerance  = mxGetField( IntegratorSettings,0,"CorrectorTolerance" );
00853         StepTuning              = mxGetField( IntegratorSettings,0,"StepSizeTuning" );
00854         LinearAlgebraSolver = mxGetField( IntegratorSettings,0,"LinearAlgebraSolver" );
00855         U                   = mxGetField( IntegratorSettings,0,"u" );
00856         P                   = mxGetField( IntegratorSettings,0,"p" );
00857         W                   = mxGetField( IntegratorSettings,0,"w" );
00858         DxInit              = mxGetField( IntegratorSettings,0,"dxInit" );
00859         SensitivityMode     = mxGetField( IntegratorSettings,0,"SensitivityMode" );
00860         Bseed               = mxGetField( IntegratorSettings,0,"mu" );
00861         Dx                  = mxGetField( IntegratorSettings,0,"lambdaX" );
00862         Du                  = mxGetField( IntegratorSettings,0,"lambdaU" );
00863         Dp                  = mxGetField( IntegratorSettings,0,"lambdaP" );
00864         Dw                  = mxGetField( IntegratorSettings,0,"lambdaW" );
00865         Bseed2              = mxGetField( IntegratorSettings,0,"mu2" );
00866         Dx2                 = mxGetField( IntegratorSettings,0,"lambda2X" );
00867         Du2                 = mxGetField( IntegratorSettings,0,"lambda2U" );
00868         Dp2                 = mxGetField( IntegratorSettings,0,"lambda2P" );
00869         Dw2                 = mxGetField( IntegratorSettings,0,"lambda2W" );
00870         PrintLevel_         = mxGetField( IntegratorSettings,0,"PrintLevel" );
00871         PlotXTrajectory     = mxGetField( IntegratorSettings,0,"PlotXTrajectory" );
00872         PlotXaTrajectory    = mxGetField( IntegratorSettings,0,"PlotXaTrajectory" );
00873         UseSubplots         = mxGetField( IntegratorSettings,0,"UseSubplots" );
00874         Jacobian                = mxGetField( IntegratorSettings,0,"Jacobian" );
00875         StorageResolution       = mxGetField( IntegratorSettings,0,"StorageResolution" );
00876 
00877         if( !mxIsEmpty(IntegratorName) )
00878         {
00879                 if ( !mxIsChar(IntegratorName) )
00880                         mexErrMsgTxt( "ERROR (ACADOintegrator): No integrator specified." );
00881 
00882                 integratorName = mxArrayToString( IntegratorName );
00883         }
00884         else
00885                 mexErrMsgTxt( "ERROR (ACADOintegrator): No integrator specified." );
00886 
00887         if( !mxIsEmpty(MaxNumStep) )
00888         {
00889                 maxNumStep = mxGetPr( MaxNumStep );
00890 
00891                 if ( isScalar( MaxNumStep ) == 0 )
00892                         mexErrMsgTxt( "ERROR (ACADOintegrator): Maximum number of steps needs to be a scalar." );
00893         }
00894 
00895         if( !mxIsEmpty(MinStep) )
00896         {
00897                 minStep = mxGetPr( MinStep );
00898 
00899                 if ( isScalar( MinStep ) == 0 )
00900                         mexErrMsgTxt( "ERROR (ACADOintegrator): Minimum step size must be a scalar ." );
00901         }
00902     
00903     if( !mxIsEmpty(InitialStep) )
00904         {
00905                 initialStep = mxGetPr( InitialStep );
00906 
00907                 if ( isScalar( InitialStep ) == 0 )
00908                         mexErrMsgTxt( "ERROR (ACADOintegrator): Initial step size must be a scalar ." );
00909         }
00910     
00911     if( !mxIsEmpty(CorrectorTolerance) )
00912         {
00913                 correctorTolerance = mxGetPr( CorrectorTolerance );
00914 
00915                 if ( isScalar( CorrectorTolerance ) == 0 )
00916                         mexErrMsgTxt( "ERROR (ACADOintegrator): Corrector Tolerance must be a scalar ." );
00917         }
00918     
00919         if( !mxIsEmpty(MaxStep) )
00920         {
00921                 maxStep = mxGetPr( MaxStep );
00922 
00923                 if ( isScalar( MaxStep ) == 0 )
00924                         mexErrMsgTxt( "ERROR (ACADOintegrator): Maximum step size must be a scalar ." );
00925         }
00926 
00927         if( !mxIsEmpty(StepTuning) )
00928         {
00929                 stepTuning = mxGetPr( StepTuning );
00930 
00931                 if ( isScalar( StepTuning ) == 0 )
00932                         mexErrMsgTxt( "ERROR (ACADOintegrator): StepTuning size must be a scalar ." );
00933         }
00934 
00935         if( !mxIsEmpty(LinearAlgebraSolver) )
00936         {
00937                 if ( !mxIsChar(LinearAlgebraSolver) )
00938                         mexErrMsgTxt( "ERROR (ACADOintegrator): No linear algebra solver specified." );
00939 
00940                 linearAlgebraSolver = mxArrayToString( LinearAlgebraSolver );
00941         }
00942         else
00943                 mexErrMsgTxt( "ERROR (ACADOintegrator): No linear algebra solver specified." );
00944 
00945         if( !mxIsEmpty(Tol) )
00946         {
00947                 tol = mxGetPr( Tol );
00948 
00949                 if ( isScalar( Tol ) == 0 )
00950                         mexErrMsgTxt( "ERROR (ACADOintegrator): Tolerance needs to be a scalar." );
00951         }
00952 
00953         if( !mxIsEmpty(aTol) )
00954         {
00955                 atol = mxGetPr( aTol );
00956 
00957                 if ( isScalar( aTol ) == 0 )
00958                         mexErrMsgTxt( "ERROR (ACADOintegrator): Absolute Tolerance needs to be a scalar." );
00959         }
00960 
00961         if( !mxIsEmpty(U) )
00962         {
00963                 u  = mxGetPr( U );
00964                 nu = mxGetM( U );
00965 
00966                 if ( mxGetN( U ) != 1 )
00967                         mexErrMsgTxt( "ERROR (ACADOintegrator): Controls need to be given as column vector." );
00968         }
00969 
00970         if( !mxIsEmpty(P) )
00971         {
00972                 p  = mxGetPr( P );
00973                 np = mxGetM( P );
00974 
00975                 if ( mxGetN( P ) != 1 )
00976                         mexErrMsgTxt( "ERROR (ACADOintegrator): Parameters need to be given as column vector." );
00977         }
00978 
00979         if( !mxIsEmpty(W) )
00980         {
00981                 w  = mxGetPr( W );
00982                 nw = mxGetM( W );
00983 
00984                 if ( mxGetN( W ) != 1 )
00985                         mexErrMsgTxt( "ERROR (ACADOintegrator): Disturbances need to be given as column vector." );
00986         }
00987 
00988         if( !mxIsEmpty(DxInit) )
00989         {
00990                 dxInit = mxGetPr( DxInit );
00991                 if ( mxGetM( DxInit ) != nx )
00992                 {
00993                         clearAllGlobals( );
00994                         mexErrMsgTxt( "ERROR (ACADOintegrator): Initial value for differential states derivatives has invalid dimensions." );
00995                 }
00996 
00997                 if ( mxGetN( DxInit ) != 1 )
00998                 {
00999                         clearAllGlobals( );
01000                         mexErrMsgTxt( "ERROR (ACADOintegrator): Initial value for differential states derivatives needs to be given as column vector." );
01001                 }
01002         }
01003 
01004         if( !mxIsEmpty(Bseed) )
01005         {
01006                 bseed = mxGetPr( Bseed );
01007 
01008                 if ( mxGetN( Bseed ) != nx )
01009                         mexErrMsgTxt( "ERROR (ACADOintegrator): Backward seed has incompatible dimensions." );
01010 
01011                 nbDir = mxGetM( Bseed );
01012         }
01013 
01014         if( !mxIsEmpty(Dx) )
01015         {
01016                 dx = mxGetPr( Dx );
01017 
01018                 if ( mxGetM( Dx ) != nx )
01019                         mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
01020 
01021                 nfDir = mxGetN( Dx );
01022         }
01023 
01024         if( !mxIsEmpty(Du) )
01025         {
01026                 du = mxGetPr( Du );
01027 
01028                 if ( mxGetM( Du ) != nx )
01029                         mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
01030 
01031                 if ( ( nfDir != 0 ) && ( nfDir != mxGetN( Du ) ) )
01032                         mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
01033 
01034                 nfDir = mxGetN( Du );
01035         }
01036 
01037         if( !mxIsEmpty(Dp) )
01038         {
01039                 dp = mxGetPr( Dp );
01040 
01041                 if ( mxGetM( Dp ) != nx )
01042                         mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
01043 
01044                 if ( ( nfDir != 0 ) && ( nfDir != mxGetN( Dp ) ) )
01045                         mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
01046 
01047                 nfDir = mxGetN( Dp );
01048         }
01049 
01050         if( !mxIsEmpty(Dw) )
01051         {
01052                 dw = mxGetPr( Dw );
01053 
01054                 if ( mxGetM( Dw ) != nx )
01055                         mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
01056 
01057                 if ( ( nfDir != 0 ) && ( nfDir != mxGetN( Dw ) ) )
01058                         mexErrMsgTxt( "ERROR (ACADOintegrator): Forward seed has incompatible dimensions." );
01059 
01060                 nfDir = mxGetN( Dw );
01061         }
01062 
01063         if( !mxIsEmpty(Bseed2) )
01064         {
01065                 bseed2 = mxGetPr( Bseed2 );
01066 
01067                 if ( mxGetN( Bseed2 ) != nx )
01068                         mexErrMsgTxt( "ERROR (ACADOintegrator): Second backward seed has incompatible dimensions." );
01069 
01070                 nbDir2 = mxGetM( Bseed2 );
01071         }
01072 
01073         if( !mxIsEmpty(Dx2) )
01074         {
01075                 dx2 = mxGetPr( Dx2 );
01076 
01077                 if ( mxGetM( Dx2 ) != nx )
01078                         mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
01079 
01080                 nfDir2  = mxGetN( Dx2 );
01081         }
01082 
01083         if( !mxIsEmpty(Du2) )
01084         {
01085                 du2 = mxGetPr( Du2 );
01086 
01087                 if ( mxGetM( Du2 ) != nx )
01088                         mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
01089 
01090                 if ( ( nfDir2 != 0 ) && ( nfDir2 != mxGetN( Du2 ) ) )
01091                         mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
01092 
01093                 nfDir2  = mxGetN( Du2 );
01094         }
01095 
01096         if( !mxIsEmpty(Dp2) )
01097         {
01098                 dp2 = mxGetPr( Dp2 );
01099 
01100                 if ( mxGetM( Dp2 ) != nx )
01101                         mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
01102 
01103                 if ( ( nfDir2 != 0 ) && ( nfDir2 != mxGetN( Dp2 ) ) )
01104                         mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
01105 
01106                 nfDir2  = mxGetN( Dp2 );
01107         }
01108 
01109         if( !mxIsEmpty(Dw2) )
01110         {
01111                 dw2 = mxGetPr( Dw2 );
01112 
01113                 if ( mxGetM( Dw2 ) != nx )
01114                         mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
01115 
01116                 if ( ( nfDir2 != 0 ) && ( nfDir2 != mxGetN( Dw2 ) ) )
01117                         mexErrMsgTxt( "ERROR (ACADOintegrator): Second forward seed has incompatible dimensions." );
01118 
01119                 nfDir2 = mxGetN( Dw2 );
01120         }
01121 
01122         if( !mxIsEmpty(PrintLevel_) )
01123                 printLevel_ = mxGetPr( PrintLevel_ );
01124 
01125 
01126         // Set link to Jacobian
01127         if ( mxIsCell(Jacobian) )
01128         {
01129                 if ( ( mxGetN( Jacobian ) == 1 ) && ( mxGetCell( Jacobian,0 ) != NULL ) )
01130                 {
01131                         ModelFcn_jac = mxDuplicateArray( mxGetCell( Jacobian,0 ) );
01132 
01133                         if ( isFunctionHandle(ModelFcn_jac) == 0 )
01134                                 mexErrMsgTxt( "ERROR (ACADOintegrator): Jacobian: No valid dynamic model specified." );
01135                 }else{
01136                         mexErrMsgTxt( "ERROR (ACADOintegrator): Jacobian: Could not set Jacobian" );
01137 
01138                 }
01139         }
01140 
01141         if( !mxIsEmpty(StorageResolution) )
01142         {
01143                 storageResolution = mxGetPr( StorageResolution );
01144 
01145                 if ( isScalar( StorageResolution ) == 0 )
01146                         mexErrMsgTxt( "ERROR (ACADOintegrator): Maximum number of steps needs to be a scalar." );
01147         }
01148 
01149 
01150 
01151         DifferentialEquation* f;
01152     f = new DifferentialEquation();
01153     
01154     CFunction *cModel = NULL;
01155     
01156 
01157         if( !mxIsEmpty(ModelName) )
01158         {
01159                 if ( mxGetM( ModelName ) != 1 )
01160                         mexErrMsgTxt( "ERROR (ACADOintegrator): No valid dynamic model specified." );
01161 
01162                 if ( mxIsChar(ModelName) )
01163                 {
01164                         modelName = mxArrayToString( ModelName );
01165                         f = allocateDifferentialEquation( modelName,integratorName );
01166 
01167                         if ( f == NULL )
01168                                 mexErrMsgTxt( "ERROR (ACADOintegrator): No valid dynamic model specified." );
01169 
01170                         if ( f->getNumAlgebraicEquations( ) != (int) nxa )
01171                                 mexErrMsgTxt( "ERROR (ACADOintegrator): Start value for algebraic states has invalid dimension." );
01172 
01173                         if ( f->getNumDynamicEquations( ) != (int) nx ){
01174                 printf("Differentialstate dimension: %d , input dim: %d \n", f->getNumDynamicEquations( ), (int)nx );
01175                 mexErrMsgTxt( "ERROR (ACADOintegrator): Start value for differential state has invalid dimension." );
01176             }
01177                 }
01178                 else
01179                 {
01180                         if ( mxIsCell(ModelName) )
01181                         {
01182                                 // get f
01183                                 if ( ( mxGetN( ModelName ) > 0 ) && ( mxGetCell( ModelName,0 ) != NULL ) )
01184                                 {
01185                                         ModelFcn_f = mxDuplicateArray( mxGetCell( ModelName,0 ) );
01186 
01187                                         if ( isFunctionHandle(ModelFcn_f) == 0 )
01188                                                 mexErrMsgTxt( "ERROR (ACADOintegrator): No valid dynamic model specified." );
01189                                 }
01190                                 else
01191                                         mexErrMsgTxt( "ERROR (ACADOintegrator): No valid dynamic model specified." );   
01192 
01193                 
01194 
01195                                 // if dynamic model is given as Matlab function handle, allocate
01196                                 // dynamic model as C function with corresponding dimensions
01197                 
01198                                 ModelFcnT  = mxCreateDoubleMatrix( 1,  1,mxREAL );
01199                                 ModelFcnX  = mxCreateDoubleMatrix( nx, 1,mxREAL );
01200                                 ModelFcnXA = mxCreateDoubleMatrix( nxa,1,mxREAL );
01201                                 ModelFcnDX = mxCreateDoubleMatrix( nx, 1,mxREAL );
01202                                 ModelFcnU  = mxCreateDoubleMatrix( nu, 1,mxREAL );
01203                                 ModelFcnP  = mxCreateDoubleMatrix( np, 1,mxREAL );
01204                                 ModelFcnW  = mxCreateDoubleMatrix( nw, 1,mxREAL );
01205                 
01206                                 modelFcnNX  = nx;
01207                                 modelFcnNXA = nxa;
01208                                 modelFcnNDX = nx;
01209                                 modelFcnNP  = np;
01210                                 modelFcnNU  = nu;
01211                                 modelFcnNW  = nw;
01212 
01213                 
01214                 IntermediateState is( 1+modelFcnNX+modelFcnNXA+modelFcnNU+modelFcnNP+modelFcnNW);
01215                 int j = 0;
01216                 
01217                 TIME t;
01218                 is(j) = t; j++;
01219                 
01220                 if (modelFcnNX > 0){
01221                     DifferentialState x("",modelFcnNX,1);
01222                     for( i=0; i<modelFcnNX; ++i ){
01223                         is(j) = x(i); j++;
01224                     }
01225                 }
01226                 
01227                 if (modelFcnNXA > 0){
01228                     AlgebraicState ax("",modelFcnNXA,1);
01229                     for( i=0; i<modelFcnNXA; ++i ){
01230                         is(j) = ax(i); j++;
01231                     }
01232                 }
01233                 
01234                 if (modelFcnNU > 0){
01235                     Control u("",modelFcnNU,1);
01236                     for( i=0; i<modelFcnNU; ++i ){
01237                         is(j) = u(i); j++;
01238                     }
01239                 }
01240                 
01241                 if (modelFcnNP > 0){
01242                     Parameter p("",modelFcnNP,1);
01243                     for( i=0; i<modelFcnNP; ++i ){
01244                         is(j) = p(i); j++;
01245                     }
01246                 }
01247                 
01248                 if (modelFcnNW > 0){
01249                     Disturbance w("",modelFcnNW,1);
01250                     for( i=0; i<modelFcnNW; ++i ){
01251                         is(j) = w(i); j++;
01252                     }
01253                 }
01254 
01255         
01256                                 //mexprintf ("nxn:%d nxa:%d np:%d nu:%d nw:%d -- j=%d\n", nx, nxa, np, nu, nw, j);
01257 
01258 
01259                                 if ( isODE == BT_TRUE )
01260                                 {
01261                                         if( !mxIsEmpty(Jacobian) )
01262                                         {
01263                         CFunction cLinkModel(nx, genericODE, genericJacobian, genericJacobian);
01264                         *f << cLinkModel(is);
01265                                         }
01266                                         else
01267                                         {
01268                         CFunction cLinkModel(nx, genericODE);
01269                         *f << cLinkModel(is);
01270                                         }
01271                                 }
01272                                 else
01273                                 {
01274                     CFunction cLinkModel(nx+nxa, genericDAE);
01275                     *f << cLinkModel(is);
01276                                 }
01277                 
01278                 
01279                 
01280                                 //printf("after link model \n");
01281                                 
01282                         }
01283                         else
01284                                 mexErrMsgTxt( "ERROR (ACADOintegrator): No dynamic model specified." );
01285                 }
01286         }
01287         else
01288                 mexErrMsgTxt( "ERROR (ACADOintegrator): No dynamic model specified." );
01289 
01290         if( !mxIsEmpty(SensitivityMode) )
01291         {
01292                 if ( !mxIsChar(SensitivityMode) )
01293                 {
01294                         delete f; delete cModel; clearAllGlobals( );
01295                         mexErrMsgTxt( "ERROR (ACADOintegrator): No valid sensitivity mode specified." );
01296                 }
01297 
01298                 sensitivityMode = mxArrayToString( SensitivityMode );
01299 
01300                 if ( ( modelName == NULL ) && ( strcmp(sensitivityMode,"AD_BACKWARD") == 0 ) )
01301                 {
01302                         delete f; delete cModel; clearAllGlobals( );
01303                         mexErrMsgTxt( "ERROR (ACADOintegrator): Adjoint sensitivity generation via function handle not possible." );
01304                 }
01305         }
01306 
01307     
01308         // set flags indicating of ODE/DAE is given and 
01309         // if output struct shall be setup
01310         if ( isODE == BT_TRUE )
01311         {
01312                 if ( nlhs == 3 )
01313                 {
01314                         delete f; delete cModel; clearAllGlobals( );
01315                         mexErrMsgTxt( "ERROR (ACADOintegrator): Invalid number of output arguments." );
01316                 }
01317 
01318                 if ( nlhs == 2 )
01319                         outputIdx = 1;
01320         }
01321         else
01322         {
01323                 if ( nlhs >= 2 )
01324                         xaEndIdx = 1;
01325 
01326                 if ( nlhs == 3 )
01327                         outputIdx = 2;
01328         }
01329 
01330 
01331         // IV. CREATE OUTPUT VECTORS AND ASSIGN POINTERS:
01332         // ----------------------------------------------
01333 
01334         double status = 0.0;
01335 
01336         plhs[0] = mxCreateDoubleMatrix( nx,1,mxREAL );
01337         xEnd    = mxGetPr( plhs[0] );
01338 
01339         if ( xaEndIdx > 0 )
01340         {
01341                 plhs[xaEndIdx] = mxCreateDoubleMatrix( nxa,1,mxREAL );
01342                 xaEnd          = mxGetPr( plhs[xaEndIdx] );
01343         }
01344 
01345         if ( outputIdx > 0 )
01346         {
01347                 // create outputs struct
01348                 // DO NOT forget to document all changes done here within integratorsOutputs.m
01349                 const char* outputFieldNames[] = {      "Status",
01350                                                                                         "NumberOfSteps",
01351                                                                                         "NumberOfRejectedSteps",
01352                                                                                         "xTrajectory",
01353                                                                                         "xaTrajectory",
01354                                                                                         "J",
01355                                                                                         "Jx",
01356                                                                                         "Ju",
01357                                                                                         "Jp",
01358                                                                                         "Jw",
01359                                                                                         "J2",
01360                                                                                         "J2x",
01361                                                                                         "J2u",
01362                                                                                         "J2p",
01363                                                                                         "J2w",
01364                                             "GetStepSize"
01365                                                                                         };
01366 
01367                 plhs[outputIdx] = mxCreateStructMatrix( 1,1,16,outputFieldNames );
01368         }
01369 
01370 
01371         // store trajectory either if output struct or if plot is desired
01372         BooleanType freezeTrajectory;
01373 
01374         if ( ( outputIdx > 0 ) || ( !mxIsEmpty( PlotXTrajectory ) ) || ( !mxIsEmpty( PlotXaTrajectory ) ) )
01375                 freezeTrajectory = BT_TRUE;
01376         else
01377                 freezeTrajectory = BT_FALSE;
01378 
01379 
01380         int *maxNumStepInt = 0;
01381         if ( !mxIsEmpty(MaxNumStep) )
01382         {
01383                 maxNumStepInt = new int;
01384                 maxNumStepInt[0] = (int) *maxNumStep;
01385         }
01386 
01387 
01388         // V. CALL THE INTEGRATION ROUTINE:
01389         // --------------------------------
01390 
01391         // allocate integrator object
01392         Integrator* integrator = allocateIntegrator( integratorName,f );
01393 
01394         if ( integrator == NULL )
01395         {
01396                 delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
01397                 mexErrMsgTxt( "ERROR (ACADOintegrator): The specified integrator has not been found.\n");
01398         }
01399 
01400         clearAllStaticCounters( );
01401 
01402 
01403         if( maxNumStep != 0 )
01404                 integrator->set( MAX_NUM_INTEGRATOR_STEPS,*maxNumStepInt );
01405 
01406     if( minStep != 0 )
01407         integrator->set( MIN_INTEGRATOR_STEPSIZE,*minStep );
01408     
01409     if( initialStep != 0 )
01410         integrator->set( INITIAL_INTEGRATOR_STEPSIZE,*initialStep );
01411             
01412     if( maxStep != 0 )
01413         integrator->set( MAX_INTEGRATOR_STEPSIZE,*maxStep );
01414 
01415         if( tol != 0 )
01416                 integrator->set(INTEGRATOR_TOLERANCE,*tol );
01417 
01418         if( atol != 0 )
01419                 integrator->set(ABSOLUTE_TOLERANCE,*atol );
01420 
01421     if( stepTuning != 0 )
01422         integrator->set( STEPSIZE_TUNING,*stepTuning );
01423 
01424     if( correctorTolerance != 0 )
01425         integrator->set( CORRECTOR_TOLERANCE,*correctorTolerance );
01426     
01427     
01428 
01429 //      integrator->set( LINEAR_ALGEBRA_SOLVER,(int)LAS_HOUSEHOLDER_METHOD );
01430 //      if( linearAlgebraSolver != 0 )
01431 //      {
01432 //              if ( strcmp(linearAlgebraSolver,"sparse") == 0 )
01433 //              {
01434 //                      if ( strcmp(integratorName,"BDF") == 0 )
01435 //                      {
01436 //                              integrator->set( LINEAR_ALGEBRA_SOLVER,(int)LAS_SPARSE_CONJUGATE_GRADIENT_METHOD );
01437 //                      }
01438 //                      else
01439 //                      {
01440 //                              delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
01441 //                              mexErrMsgTxt( "ERROR (ACADOintegrator): The specified integrator has not been found.\n");
01442 //                      }
01443 //              }
01444 //      }
01445 
01446 
01447         if( dxInit != NULL )
01448                 integrator->setDxInitialization( dxInit );
01449 
01450         if( freezeTrajectory == BT_TRUE )
01451                 integrator->freezeAll( );
01452 
01453     //printf("before integrate\n");
01454 
01455     Grid timeInterval( *tStart, *tEnd, (int) *storageResolution );
01456         returnValue returnvalue = integrator->integrate( timeInterval, xStart,xaStart,p,u,w );
01457     
01458     //printf("after integrate \n");
01459     
01460         if ( returnvalue != SUCCESSFUL_RETURN )
01461         {
01462                 status = -1.0;
01463                 plotErrorMessage( returnvalue,printLevel_ );
01464         }
01465 
01466         // assign end value for states
01467         //double* xEndFull = new double[nx+nxa];
01468     DVector xEnd_, xaEnd_;
01469         integrator->getX ( xEnd_  );
01470         integrator->getXA( xaEnd_ );
01471 
01472         for( i=0; i<nx; ++i )
01473                 xEnd[i] = xEnd_(i);
01474 
01475         if ( xaEndIdx > 0 )
01476         {
01477                 for( i=0; i<nxa; ++i )
01478                         xaEnd[i] = xaEnd_(i);
01479         }
01480 
01481     //printf("after xaEnd, before sensitivities \n");
01482 
01483         // SENSITIVITIES:
01484         // -----------------------------------
01485         if ( ( !mxIsEmpty(SensitivityMode) ) && ( status >= 0.0 ) )
01486         {
01487         //printf("\n !! SensitivityMode is currently disabled !! \n\n");
01488                 // forward mode
01489                 if( strcmp(sensitivityMode,"AD_FORWARD") == 0  )
01490                 {
01491                         if ( ( dx == NULL ) && ( du == NULL ) && ( dp == NULL ) && ( dw == NULL ) )
01492                         {
01493                                 delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
01494                                 mexErrMsgTxt( "ERROR (ACADOintegrator): The forward seed is not defined." );
01495                         }
01496 
01497                         // setup seed and sensitvity matrices
01498                         DMatrix D_x( nx,nfDir );
01499                         DMatrix D_p( np,nfDir );
01500                         DMatrix D_u( nu,nfDir );
01501                         DMatrix D_w( nw,nfDir );
01502 
01503                         if( dx != NULL ) {
01504                 D_x = Eigen::Map<DMatrix>(dx,nx,nfDir);
01505                 D_x.transposeInPlace();
01506             }
01507                         else
01508                                 D_x.setZero();
01509 
01510                         if( du != NULL ) {
01511                 D_u = Eigen::Map<DMatrix>(du,nu,nfDir);
01512                 D_u.transposeInPlace();
01513             }
01514                         else
01515                                 D_u.setZero();
01516 
01517                         if( dp != NULL ) {
01518                 D_p = Eigen::Map<DMatrix>(dp,np,nfDir);
01519                 D_p.transposeInPlace();
01520             }
01521                         else
01522                                 D_p.setZero();
01523 
01524                         if( dw != NULL ) {
01525                 D_w = Eigen::Map<DMatrix>(dw,nw,nfDir);
01526                 D_w.transposeInPlace();
01527             }
01528                         else
01529                                 D_w.setZero();
01530 
01531                         DMatrix J( nx+nxa,nfDir );
01532 
01533                         // determine forward sensitivities
01534 
01535             int run1;
01536 
01537             for( run1 = 0; run1 < nfDir; run1++ ){
01538 
01539                 DVector DXX = D_x.getCol( run1 );
01540                 DVector DPP = D_p.getCol( run1 );
01541                 DVector DUU = D_u.getCol( run1 );
01542                 DVector DWW = D_w.getCol( run1 );
01543 
01544                                 integrator->setForwardSeed( 1, DXX, DPP, DUU, DWW );
01545                                 returnvalue = integrator->integrateSensitivities( );
01546                                 if( returnvalue != SUCCESSFUL_RETURN )
01547                                 {
01548                                         status = -1.0;
01549                                         plotErrorMessage( returnvalue,printLevel_ );
01550                                 }
01551                                 else
01552                                 {
01553                     DVector JJ(nx+nxa);
01554                                         integrator->getForwardSensitivities( JJ, 1 );
01555                     J.setCol( run1, JJ );
01556                 }
01557             }
01558 
01559                         // write sensitivity matrices into output struct (if given)
01560                         if ( returnvalue == SUCCESSFUL_RETURN && outputIdx > 0 )
01561                         {
01562                                 JJ = mxCreateDoubleMatrix( nx+nxa,nfDir,mxREAL );
01563                                 jj = mxGetPr( JJ );
01564                 DMatrix jj_tmp = Eigen::Map<DMatrix>(jj, J.rows(), J.cols());  jj_tmp = J.transpose(); jj = jj_tmp.data();
01565                                 mxSetField( plhs[outputIdx],0,"J",JJ );
01566                         }
01567                 }
01568 
01569                 // backward mode
01570                 if( strcmp(sensitivityMode,"AD_BACKWARD") == 0 )
01571                 {
01572                         if( bseed == NULL )
01573                         {
01574                                 delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
01575                                 mexErrMsgTxt( "ERROR (ACADOintegrator): The backward seed is not defined." );
01576                         }
01577 
01578                         // setup seed and sensitvity matrices
01579                         DMatrix xSeed( nbDir,nx );
01580             xSeed = Eigen::Map<DMatrix>(bseed,nbDir,nx);
01581             xSeed.transposeInPlace();
01582 
01583                         DMatrix J_x( nbDir,nx+nxa );
01584                         DMatrix J_u( nbDir,nu );
01585                         DMatrix J_p( nbDir,np );
01586                         DMatrix J_w( nbDir,nw );
01587 
01588 
01589             int run1;
01590             for( run1 = 0; run1 < nbDir; run1++ ){
01591 
01592                 DVector XSEED = xSeed.getRow(run1);
01593 
01594                                 // determine backward sensitivities
01595                                 integrator->setBackwardSeed( 1, XSEED );
01596                                 returnvalue = integrator->integrateSensitivities( );
01597                                 if( returnvalue != SUCCESSFUL_RETURN )
01598                                 {
01599                                         status = -1.0;
01600                                         plotErrorMessage( returnvalue,printLevel_ );
01601                                 }
01602                                 else
01603                                 {
01604                     DVector JXX(nx+nxa), JPP(np), JUU(nu), JWW(nw);
01605 
01606                                         integrator->getBackwardSensitivities( JXX, JPP, JUU, JWW, 1 );
01607 
01608                     J_x.setRow( run1, JXX );
01609                     J_p.setRow( run1, JPP );
01610                     J_u.setRow( run1, JUU );
01611                     J_w.setRow( run1, JWW );
01612                     }
01613             }
01614 
01615 
01616             if( returnvalue == SUCCESSFUL_RETURN ){
01617                                 // write sensitivity matrices into output struct
01618                                 if ( outputIdx > 0 )
01619                                 {
01620                                         Jx = mxCreateDoubleMatrix( nbDir,nx+nxa,mxREAL );
01621                                         jx = mxGetPr( Jx );
01622                     DMatrix jx_tmp = Eigen::Map<DMatrix>(jx, J_x.rows(), J_x.cols());  jx_tmp = J_x.transpose(); jx = jx_tmp.data();
01623                                         mxSetField( plhs[outputIdx],0,"Jx",Jx );
01624 
01625                                         if ( nu > 0 )
01626                                         {
01627                                                 Ju = mxCreateDoubleMatrix( nbDir,nu,mxREAL );
01628                                                 ju = mxGetPr( Ju );
01629                         DMatrix ju_tmp = Eigen::Map<DMatrix>(ju, J_u.rows(), J_u.cols());  ju_tmp = J_u.transpose(); ju = ju_tmp.data();
01630                                                 mxSetField( plhs[outputIdx],0,"Ju",Ju );
01631                                         }
01632 
01633                                         if( np > 0 )
01634                                         {
01635                                                 Jp = mxCreateDoubleMatrix( nbDir,np,mxREAL );
01636                                                 jp = mxGetPr( Jp );
01637                         DMatrix jp_tmp = Eigen::Map<DMatrix>(jp, J_p.rows(), J_p.cols());  jp_tmp = J_p.transpose(); jp = jp_tmp.data();
01638                                                 mxSetField( plhs[outputIdx],0,"Jp",Jp );
01639                                         }
01640 
01641                                         if( nw > 0 )
01642                                         {
01643                                                 Jw = mxCreateDoubleMatrix( nbDir,nw,mxREAL );
01644                                                 jw = mxGetPr( Jw );
01645                         DMatrix jw_tmp = Eigen::Map<DMatrix>(jw, J_w.rows(), J_w.cols());  jw_tmp = J_w.transpose(); jw = jw_tmp.data();
01646                                                 mxSetField( plhs[outputIdx],0,"Jw",Jw );
01647                                         }
01648                                 }
01649                         }
01650                 }
01651 
01652 
01653                 // forward mode (2nd derivatives)
01654                 if ( strcmp(sensitivityMode,"AD_FORWARD2") == 0  )
01655                 {
01656                         if ( ( dx == NULL ) && ( du == NULL ) && ( dp == NULL ) && ( dw == NULL ) )
01657                         {
01658                                 delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
01659                                 mexErrMsgTxt( "ERROR (integrator): The forward seed is not defined." );
01660                         }
01661 
01662                         if ( ( dx2 == NULL ) && ( du2 == NULL ) && ( dp2 == NULL ) && ( dw2 == NULL ) )
01663                         {
01664                                 delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
01665                                 mexErrMsgTxt( "ERROR (integrator): The second order forward seed is not defined." );
01666                         }
01667 
01668                         if ( nfDir > 1 )
01669                         {
01670                                 delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
01671                                 mexErrMsgTxt( "ERROR (integrator): More than one first order seed is not allowed in second order mode - please compute the required directions in a loop." );
01672                         }
01673 
01674                         // setup seed and sensitvity matrices (first order)
01675                         DMatrix D_x( nx,nfDir );
01676                         DMatrix D_p( np,nfDir );
01677                         DMatrix D_u( nu,nfDir );
01678                         DMatrix D_w( nw,nfDir );
01679 
01680                         if( dx != NULL ) {
01681                 D_x = Eigen::Map<DMatrix>(dx,nx,nfDir);
01682                 D_x.transposeInPlace();
01683             }
01684                         else
01685                                 D_x.setZero();
01686 
01687                         if( du != NULL ) {
01688                 D_u = Eigen::Map<DMatrix>(du,nu,nfDir);
01689                 D_u.transposeInPlace();
01690             }
01691                         else
01692                                 D_u.setZero();
01693 
01694                         if( dp != NULL ) {
01695                 D_p = Eigen::Map<DMatrix>(dp,np,nfDir);
01696                 D_p.transposeInPlace();
01697             }
01698                         else
01699                                 D_p.setZero();
01700 
01701                         if( dw != NULL ) {
01702                 D_w = Eigen::Map<DMatrix>(dw,nw,nfDir);
01703                 D_w.transposeInPlace();
01704             }
01705                         else
01706                                 D_w.setZero();
01707 
01708                         DMatrix J( nx+nxa,nfDir );
01709 
01710                         // setup seed and sensitvity matrices (second order)
01711                         DMatrix D_x2( nx,nfDir2 );
01712                         DMatrix D_p2( np,nfDir2 );
01713                         DMatrix D_u2( nu,nfDir2 );
01714                         DMatrix D_w2( nw,nfDir2 );
01715 
01716                         if( dx != NULL ) {
01717                 D_x2 = Eigen::Map<DMatrix>(dx2,nx,nfDir2);
01718                 D_x2.transposeInPlace();
01719             }
01720                         else
01721                                 D_x2.setZero();
01722 
01723                         if( du != NULL ) {
01724                 D_u2 = Eigen::Map<DMatrix>(du2,nu,nfDir2);
01725                 D_u2.transposeInPlace();
01726             }
01727                         else
01728                                 D_u2.setZero();
01729 
01730                         if( dp2 != NULL ) {
01731                 D_p2 = Eigen::Map<DMatrix>(dp2,np,nfDir2);
01732                 D_p2.transposeInPlace();
01733             }
01734                         else
01735                                 D_p2.setZero();
01736 
01737                         if( dw2 != NULL ) {
01738                 D_w2 = Eigen::Map<DMatrix>(dw2,nw,nfDir2);
01739                 D_w2.transposeInPlace();
01740             }
01741                         else
01742                                 D_w2.setZero();
01743 
01744                         DMatrix J2( nx+nxa,nfDir2 );
01745 
01746 
01747             int run1, run2;
01748 
01749             for( run1 = 0; run1 < nfDir; run1++ ){
01750 
01751                 DVector DXX = D_x.getCol( run1 );
01752                 DVector DPP = D_p.getCol( run1 );
01753                 DVector DUU = D_u.getCol( run1 );
01754                 DVector DWW = D_w.getCol( run1 );
01755 
01756                                 integrator->setForwardSeed( 1, DXX, DPP, DUU, DWW );
01757                                 returnvalue = integrator->integrateSensitivities( );
01758 
01759                                 if( returnvalue != SUCCESSFUL_RETURN )
01760                                 {
01761                                         status = -1.0;
01762                                         plotErrorMessage( returnvalue,printLevel_ );
01763                                 }
01764                                 else
01765                                 {
01766                     DVector JJ(nx+nxa);
01767                                         integrator->getForwardSensitivities( JJ, 1 );
01768                     J.setCol( run1, JJ );
01769                 }
01770 
01771                 for( run2 = 0; run2 < nfDir2; run2++ ){
01772 
01773                     DVector DXX2 = D_x2.getCol( run2 );
01774                     DVector DPP2 = D_p2.getCol( run2 );
01775                     DVector DUU2 = D_u2.getCol( run2 );
01776                     DVector DWW2 = D_w2.getCol( run2 );
01777 
01778                                         // determine forward sensitivities
01779                                         integrator->setForwardSeed( 2, DXX2, DPP2, DUU2, DWW2 );
01780                                         returnvalue = integrator->integrateSensitivities( );
01781 
01782                                         if( returnvalue != SUCCESSFUL_RETURN )
01783                                         {
01784                                                 status = -1.0;
01785                                                 plotErrorMessage( returnvalue,printLevel_ );
01786                                         }
01787                                         else
01788                                         {
01789                         DVector JJ2(nx+nxa);
01790                                         integrator->getForwardSensitivities( JJ2, 2 );
01791                         J2.setCol( run2, JJ2 );
01792                     }
01793                 }
01794             }
01795 
01796                         // write sensitivity matrices into output struct (if given)
01797                         if ( returnvalue == SUCCESSFUL_RETURN && outputIdx > 0 )
01798                         {
01799                                 JJ = mxCreateDoubleMatrix( nx+nxa,nfDir,mxREAL );
01800                                 jj = mxGetPr( JJ );
01801                 DMatrix jj_tmp = Eigen::Map<DMatrix>(jj, J.rows(), J.cols());  jj_tmp = J.transpose(); jj = jj_tmp.data();
01802                                 mxSetField( plhs[outputIdx],0,"J",JJ );
01803 
01804                                 JJ2 = mxCreateDoubleMatrix( nx+nxa,nfDir2,mxREAL );
01805                                 jj2 = mxGetPr( JJ2 );
01806                 DMatrix jj2_tmp = Eigen::Map<DMatrix>(jj2, J2.rows(), J2.cols());  jj2_tmp = J2.transpose(); jj2 = jj2_tmp.data();
01807                                 mxSetField( plhs[outputIdx],0,"J2",JJ2 );
01808                         }
01809                 }
01810 
01811 
01812                 // mixed forward-backward mode (2nd derivatives)
01813                 if ( strcmp(sensitivityMode,"AD_FORWARD_BACKWARD") == 0 )
01814                 {
01815                         if ( ( dx == NULL ) && ( du == NULL ) && ( dp == NULL ) && ( dw == NULL ) )
01816                         {
01817                                 delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
01818                                 mexErrMsgTxt( "ERROR (integrator): The forward seed is not defined." );
01819                         }
01820 
01821                         if( bseed2 == NULL )
01822                         {
01823                                 delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
01824                                 mexErrMsgTxt( "ERROR (integrator): The second order backward seed is not defined." );
01825                         }
01826 
01827                         if( nfDir > 1 )
01828                         {
01829                                 delete integrator; delete maxNumStepInt; delete f; delete cModel; clearAllGlobals( );
01830                                 mexErrMsgTxt( "ERROR (integrator): More than one first order seed is not allowed in second order mode - please compute the required directions in a loop." );
01831                         }
01832 
01833                         // setup seed and sensitvity matrices (first order)
01834                         DMatrix D_x( nx,nfDir );
01835                         DMatrix D_p( np,nfDir );
01836                         DMatrix D_u( nu,nfDir );
01837                         DMatrix D_w( nw,nfDir );
01838 
01839                         if( dx != NULL ) {
01840                 D_x = Eigen::Map<DMatrix>(dx,nx,nfDir);
01841                 D_x.transposeInPlace();
01842             }
01843                         else
01844                                 D_x.setZero();
01845 
01846                         if( du != NULL ) {
01847                 D_u = Eigen::Map<DMatrix>(du,nu,nfDir);
01848                 D_u.transposeInPlace();
01849             }
01850                         else
01851                                 D_u.setZero();
01852 
01853                         if( dp != NULL ) {
01854                 D_p = Eigen::Map<DMatrix>(dp,np,nfDir);
01855                 D_p.transposeInPlace();
01856             }
01857                         else
01858                                 D_p.setZero();
01859 
01860                         if( dw != NULL ) {
01861                 D_w = Eigen::Map<DMatrix>(dw,nw,nfDir);
01862                 D_w.transposeInPlace();
01863             }
01864                         else
01865                                 D_w.setZero();
01866 
01867                         DMatrix J( nx+nxa,nfDir );
01868 
01869                         // setup seed and sensitvity matrices
01870                         DMatrix bSeed2( nbDir2,nx );
01871             bSeed2 = Eigen::Map<DMatrix>(bseed2,nbDir2,nx);
01872             bSeed2.transposeInPlace();
01873 
01874                         DMatrix J_x2( nbDir2,nx+nxa );
01875                         DMatrix J_u2( nbDir2,nu );
01876                         DMatrix J_p2( nbDir2,np );
01877                         DMatrix J_w2( nbDir2,nw );
01878 
01879 
01880             int run1, run2;
01881 
01882             for( run1 = 0; run1 < nfDir; run1++ ){
01883 
01884                 DVector DXX = D_x.getCol( run1 );
01885                 DVector DPP = D_p.getCol( run1 );
01886                 DVector DUU = D_u.getCol( run1 );
01887                 DVector DWW = D_w.getCol( run1 );
01888 
01889                                 integrator->setForwardSeed( 1, DXX, DPP, DUU, DWW );
01890                                 returnvalue = integrator->integrateSensitivities( );
01891 
01892                                 if( returnvalue != SUCCESSFUL_RETURN )
01893                                 {
01894                                         status = -1.0;
01895                                         plotErrorMessage( returnvalue,printLevel_ );
01896                                 }
01897                                 else
01898                                 {
01899                     DVector JJ(nx+nxa);
01900                                         integrator->getForwardSensitivities( JJ, 1 );
01901                     J.setCol( run1, JJ );
01902                 }
01903 
01904                 for( run2 = 0; run2 < nbDir2; run2++ ){
01905 
01906                     DVector XSEED = bSeed2.getRow(run2);
01907 
01908                                         // determine backward sensitivities
01909                                         integrator->setBackwardSeed( 2, XSEED );
01910                                         returnvalue = integrator->integrateSensitivities( );
01911                                         if( returnvalue != SUCCESSFUL_RETURN )
01912                                         {
01913                                                 status = -1.0;
01914                                                 plotErrorMessage( returnvalue,printLevel_ );
01915                                         }
01916                                         else
01917                                         {
01918                             DVector JXX2(nx+nxa), JPP2(np), JUU2(nu), JWW2(nw);
01919 
01920                                                 integrator->getBackwardSensitivities( JXX2, JPP2, JUU2, JWW2, 2 );
01921 
01922                             J_x2.setRow( run2, JXX2 );
01923                         J_p2.setRow( run2, JPP2 );
01924                         J_u2.setRow( run2, JUU2 );
01925                         J_w2.setRow( run2, JWW2 );
01926                         }
01927                 }
01928             }
01929 
01930 
01931                         // write sensitivity matrices into output struct (if given)
01932                         if ( returnvalue == SUCCESSFUL_RETURN && outputIdx > 0 )
01933                         {
01934                                 JJ = mxCreateDoubleMatrix( nx+nxa,nfDir,mxREAL );
01935                                 jj = mxGetPr( JJ );
01936                 DMatrix jj_tmp = Eigen::Map<DMatrix>(jj, J.rows(), J.cols());  jj_tmp = J.transpose(); jj = jj_tmp.data();
01937                                 mxSetField( plhs[outputIdx],0,"J",JJ );
01938 
01939                                 Jx2 = mxCreateDoubleMatrix( nbDir2,nx+nxa,mxREAL );
01940                                 jx2 = mxGetPr( Jx2 );
01941                 DMatrix jx2_tmp = Eigen::Map<DMatrix>(jx2, J_x2.rows(), J_x2.cols());  jx2_tmp = J_x2.transpose(); jx2 = jx2_tmp.data();
01942                                 mxSetField( plhs[outputIdx],0,"J2x",Jx2 );
01943 
01944                                 if ( nu > 0 )
01945                                 {
01946                                         Ju2 = mxCreateDoubleMatrix( nbDir2,nu,mxREAL );
01947                                         ju2 = mxGetPr( Ju2 );
01948                     DMatrix ju2_tmp = Eigen::Map<DMatrix>(ju2, J_u2.rows(), J_u2.cols());  ju2_tmp = J_u2.transpose(); ju2 = ju2_tmp.data();
01949                                         mxSetField( plhs[outputIdx],0,"J2u",Ju2 );
01950                                 }
01951 
01952                                 if( np > 0 )
01953                                 {
01954                                         Jp2 = mxCreateDoubleMatrix( nbDir2,np,mxREAL );
01955                                         jp2 = mxGetPr( Jp2 );
01956                     DMatrix jp2_tmp = Eigen::Map<DMatrix>(jp2, J_p2.rows(), J_p2.cols());  jp2_tmp = J_p2.transpose(); jp2 = jp2_tmp.data();
01957                                         mxSetField( plhs[outputIdx],0,"J2p",Jp2 );
01958                                 }
01959 
01960                                 if( nw > 0 )
01961                                 {
01962                                         Jw2 = mxCreateDoubleMatrix( nbDir2,nw,mxREAL );
01963                                         jw2 = mxGetPr( Jw2 );
01964                     DMatrix jw2_tmp = Eigen::Map<DMatrix>(jw2, J_w2.rows(), J_w2.cols());  jw2_tmp = J_w2.transpose(); jw2 = jw2_tmp.data();
01965                                         mxSetField( plhs[outputIdx],0,"J2w",Jw2 );
01966                                 }
01967                         }
01968                 }
01969         }
01970 
01971 
01972         // VI. STORE OUTPUT STRUCT:
01973         // ------------------------
01974 
01975         if ( ( outputIdx > 0 ) || ( !mxIsEmpty( PlotXTrajectory ) ) || ( !mxIsEmpty( PlotXaTrajectory ) ) )
01976         {
01977                 Status = mxCreateDoubleMatrix( 1,1,mxREAL );
01978                 statusPtr = mxGetPr( Status );
01979                 statusPtr[0] = status;
01980 
01981                 if ( status >= 0.0 )
01982                 {
01983             
01984             // Get x
01985             VariablesGrid out_x;
01986             integrator->getX(out_x);
01987             
01988             XTrajectory = mxCreateDoubleMatrix( out_x.getNumPoints(),1+out_x.getNumValues(),mxREAL );
01989             xTrajectory = mxGetPr( XTrajectory );
01990 
01991             for( int i=0; i<out_x.getNumPoints(); ++i ){ 
01992                 xTrajectory[0*out_x.getNumPoints() + i] = out_x.getTime(i);
01993                 for( int j=0; j<out_x.getNumValues(); ++j ){
01994                     xTrajectory[(1+j)*out_x.getNumPoints() + i] = out_x(i, j);
01995                 }
01996             }
01997 
01998             
01999             // Get xa
02000             if ( isODE == BT_FALSE ){
02001 
02002                 VariablesGrid out_xa;
02003                 integrator->getXA(out_xa);
02004 
02005                 XaTrajectory = mxCreateDoubleMatrix( out_xa.getNumPoints(),1+out_xa.getNumValues(),mxREAL );
02006                 xaTrajectory = mxGetPr( XaTrajectory );
02007 
02008                 for( int i=0; i<out_xa.getNumPoints(); ++i ){ 
02009                     xaTrajectory[0*out_xa.getNumPoints() + i] = out_xa.getTime(i);
02010                     for( int j=0; j<out_xa.getNumValues(); ++j ){
02011                         xaTrajectory[(1+j)*out_xa.getNumPoints() + i] = out_xa(i, j);
02012                     } 
02013                 }
02014             }
02015 
02016 
02017                         NumberOfSteps = mxCreateDoubleMatrix( 1,1,mxREAL );
02018                         numberOfSteps = mxGetPr( NumberOfSteps );
02019                         *numberOfSteps = (double) integrator->getNumberOfSteps();
02020 
02021                         NumberOfRejectedSteps = mxCreateDoubleMatrix( 1,1,mxREAL );
02022                         numberOfRejectedSteps = mxGetPr( NumberOfRejectedSteps );
02023                         *numberOfRejectedSteps = (double) integrator->getNumberOfRejectedSteps();
02024 
02025             GetStepSize = mxCreateDoubleMatrix( 1,1,mxREAL );
02026                         getStepSize = mxGetPr( GetStepSize );
02027                         *getStepSize = (double) integrator->getStepSize();  
02028 
02029             
02030             
02031                         // plot if desired
02032                         mxArray *PlotType = mxCreateDoubleMatrix( 1,1,mxREAL );
02033                         double  *plotType = mxGetPr( PlotType );
02034 
02035                         mxArray *IsDiscretized = mxCreateDoubleMatrix( 1,1,mxREAL );
02036                         double  *isDiscretized = mxGetPr( IsDiscretized );
02037 
02038                         if( strcmp(integratorName,"DiscretizedODE") == 0 )
02039                                 isDiscretized[0] = 1.0;
02040                         else
02041                                 isDiscretized[0] = 0.0;
02042 
02043             
02044                         if ( !mxIsEmpty( PlotXTrajectory ) )
02045                         {
02046                                 plotType[0] = 0.0;
02047                                 mxArray* plotArguments[] = {    XTrajectory,PlotXTrajectory,UseSubplots,
02048                                                                                                 PlotType,IsDiscretized };
02049                                 mexCallMATLAB( 0,0,5,plotArguments,"plot_trajectory" );
02050                         }
02051 
02052                         if ( ( isODE == BT_FALSE ) && ( !mxIsEmpty( PlotXaTrajectory ) ) )
02053                         {
02054                                 plotType[0] = 1.0;
02055                                 mxArray* plotArguments[] = {    XaTrajectory,PlotXaTrajectory,UseSubplots,
02056                                                                                                 PlotType,IsDiscretized };
02057                                 mexCallMATLAB( 0,0,5,plotArguments,"plot_trajectory" );
02058                         }
02059 
02060                         mxDestroyArray( IsDiscretized );
02061                         mxDestroyArray( PlotType );
02062                         PlotType = NULL;
02063                         plotType = NULL;
02064         
02065 
02066                         // write output struct if given
02067                         if ( outputIdx > 0 )
02068                         {
02069                                 mxSetField( plhs[outputIdx],0,"Status",Status );
02070                                 mxSetField( plhs[outputIdx],0,"NumberOfSteps",NumberOfSteps );
02071                                 mxSetField( plhs[outputIdx],0,"NumberOfRejectedSteps",NumberOfRejectedSteps );
02072                                 mxSetField( plhs[outputIdx],0,"xTrajectory",XTrajectory );
02073                                 mxSetField( plhs[outputIdx],0,"GetStepSize",GetStepSize );
02074                 
02075                                 if ( isODE == BT_FALSE )
02076                                         mxSetField( plhs[outputIdx],0,"xaTrajectory",XaTrajectory );
02077                         }
02078 
02079                 }
02080                 else
02081                 {
02082                         if ( outputIdx > 0 )
02083                                 mxSetField( plhs[outputIdx],0,"Status",Status );
02084                 }
02085         }
02086     
02087     
02088         // VII. FREE THE MEMORY:
02089         // ---------------------
02090 
02091         delete integrator;
02092         delete f;
02093     delete cModel; 
02094     
02095         if ( modelName != NULL )
02096                 mxFree(modelName);
02097 
02098         if( !mxIsEmpty(IntegratorName) )
02099                 mxFree(integratorName);
02100 
02101         if( !mxIsEmpty(SensitivityMode) )
02102                 mxFree(sensitivityMode);
02103 
02104         if ( !mxIsEmpty(MaxNumStep) )
02105                 delete maxNumStepInt;
02106 
02107         clearAllGlobals( );
02108 }


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:36:41