/*! \fn updateContinuousSystem
*
* Function to update the whole system with EventIteration.
* Evaluate the functionDAE()
*
* \param [ref] [data]
*/
static void prefixedName_updateContinuousSystem(DATA *data, threadData_t *threadData)
{
TRACE_PUSH
externalInputUpdate(data);
data->callback->input_function(data, threadData);
data->callback->functionODE(data, threadData);
data->callback->functionAlgebraics(data, threadData);
data->callback->output_function(data, threadData);
data->callback->function_storeDelayed(data, threadData);
storePreValues(data);
TRACE_POP
}
static modelica_boolean LIQSS2_calculateState(DATA* data, threadData_t *threadData){
externalInputUpdate(data);
data->callback->input_function(data, threadData);
data->callback->functionODE(data, threadData);
data->callback->functionAlgebraics(data, threadData);
data->callback->output_function(data, threadData);
data->callback->function_storeDelayed(data, threadData);
data->callback->functionDAE(data, threadData);
if(data->simulationInfo->needToIterate){
//printf("needToIterate is een\n");
//printf("nTI: %d\n", data->simulationInfo->needToIterate);
return true;
}
else
return false;
}
/*! \fn updateContinuousSystem
*
* Function to update the whole system with EventIteration.
* Evaluate the functionDAE()
*
* \param [ref] [data]
*/
static void prefixedName_updateContinuousSystem(DATA *data, threadData_t *threadData)
{
TRACE_PUSH
externalInputUpdate(data);
data->callback->input_function(data, threadData);
if (compiledInDAEMode) /* dae mode */
{
data->simulationInfo->daeModeData->evaluateDAEResiduals(data, threadData, EVAL_ALGEBRAIC);
}
else /* ode mode */
{
data->callback->functionODE(data, threadData);
data->callback->functionAlgebraics(data, threadData);
}
data->callback->output_function(data, threadData);
data->callback->setc_function(data, threadData);
data->callback->function_storeDelayed(data, threadData);
storePreValues(data);
TRACE_POP
}
/*! performQSSSimulation(DATA* data, SOLVER_INFO* solverInfo)
*
* \param [ref] [data]
* \param [ref] [solverInfo]
*
* This function performs the simulation controlled by solverInfo.
*/
modelica_integer prefixedName_performQSSSimulation(DATA* data, SOLVER_INFO* solverInfo)
{
TRACE_PUSH
SIMULATION_INFO *simInfo = &(data->simulationInfo);
MODEL_DATA *mData = &(data->modelData);
uinteger currStepNo = 0;
modelica_integer retValIntegrator = 0;
modelica_integer retValue = 0;
uinteger ind = 0;
solverInfo->currentTime = simInfo->startTime;
warningStreamPrint(LOG_STDOUT, 0, "This QSS method is under development and should not be used yet.");
if (data->callback->initialAnalyticJacobianA(data))
{
infoStreamPrint(LOG_STDOUT, 0, "Jacobian or sparse pattern is not generated or failed to initialize.");
return UNKNOWN;
}
printSparseStructure(data, LOG_SOLVER);
/* *********************************************************************************** */
/* Initialization */
uinteger i = 0; /* loop var */
SIMULATION_DATA *sData = (SIMULATION_DATA*)data->localData[0];
modelica_real* state = sData->realVars;
modelica_real* stateDer = sData->realVars + data->modelData.nStates;
const SPARSE_PATTERN* pattern = &(data->simulationInfo.analyticJacobians[data->callback->INDEX_JAC_A].sparsePattern);
const uinteger ROWS = data->simulationInfo.analyticJacobians[data->callback->INDEX_JAC_A].sizeRows;
const uinteger STATES = data->modelData.nStates;
uinteger numDer = 0; /* number of derivatives influenced by state k */
modelica_boolean fail = 0;
modelica_real* qik = NULL; /* Approximation of states */
modelica_real* xik = NULL; /* states */
modelica_real* derXik = NULL; /* Derivative of states */
modelica_real* tq = NULL; /* Time of approximation, because not all approximations are calculated at a specific time, each approx. has its own timestamp */
modelica_real* tx = NULL; /* Time of the states, because not all states are calculated at a specific time, each state has its own timestamp */
modelica_real* tqp = NULL; /* Time of the next change in state */
modelica_real* nQh = NULL; /* next value of the state */
modelica_real* dQ = NULL; /* change in quantity of every state, default = nominal*10^-4 */
/* allocate memory*/
qik = (modelica_real*)calloc(STATES, sizeof(modelica_real));
fail = (qik == NULL) ? 1 : ( 0 | fail);
xik = (modelica_real*)calloc(STATES, sizeof(modelica_real));
fail = (xik == NULL) ? 1 : ( 0 | fail);
derXik = (modelica_real*)calloc(STATES, sizeof(modelica_real));
fail = (derXik == NULL) ? 1 : ( 0 | fail);
tq = (modelica_real*)calloc(STATES, sizeof(modelica_real));
fail = (tq == NULL) ? 1 : ( 0 | fail);
tx = (modelica_real*)calloc(STATES, sizeof(modelica_real));
fail = (tx == NULL) ? 1 : ( 0 | fail);
tqp = (modelica_real*)calloc(STATES, sizeof(modelica_real));
fail = (tqp == NULL) ? 1 : ( 0 | fail);
nQh = (modelica_real*)calloc(STATES, sizeof(modelica_real));
fail = (nQh == NULL) ? 1 : ( 0 | fail);
dQ = (modelica_real*)calloc(STATES, sizeof(modelica_real));
fail = (dQ == NULL) ? 1 : ( 0 | fail);
if (fail)
return OO_MEMORY;
/* end - allocate memory */
/* further initialization of local variables */
modelica_real diffQ = 0.0, dTnextQ = 0.0, nextQ = 0.0;
for (i = 0; i < STATES; i++)
{
dQ[i] = 0.0001 * data->modelData.realVarsData[i].attribute.nominal;
tx[i] = tq[i] = simInfo->startTime;
qik[i] = state[i];
xik[i] = state[i];
derXik[i] = stateDer[i];
retValue = deltaQ(data, dQ[i], i, &dTnextQ, &nextQ, &diffQ);
if (OK != retValue)
return retValue;
tqp[i] = tq[i] + dTnextQ;
nQh[i] = nextQ;
}
/* Transform the sparsity pattern into a data structure for an index based access. */
modelica_integer* der = (modelica_integer*)calloc(ROWS, sizeof(modelica_integer));
if (NULL==der)
return OO_MEMORY;
for (i = 0; i < ROWS; i++)
der[i] = -1;
/* how many states are involved in each derivative */
/* **** This is needed if we have QSS2 or higher **** */
/* uinteger* numStatesInDer = calloc(ROWS,sizeof(uinteger));
if (NULL==numStatesInDer) return OO_MEMORY; */
/*
* dx1/dt = x2
* dx2/dt = x1 + x2
* lead to
* StatesInDer[0]-{ 2 }
* StatesInDer[1]-{ 1, 2 }
*/
/* uinteger** StatesInDer = calloc(ROWS,sizeof(uinteger*));
if (NULL==StatesInDer) return OO_MEMORY; */
/* count number of states in each derivative*/
/* for (i = 0; i < pattern->leadindex[ROWS-1]; i++) numStatesInDer[ pattern->index[i] ]++; */
/* collect memory for all stateindices */
/* for (i = 0; i < ROWS; i++)
{
*(StatesInDer + i) = calloc(numStatesInDer[i], sizeof(uinteger));
if (NULL==*(StatesInDer + i)) return OO_MEMORY;
}
retValue = getStatesInDer(pattern->index, pattern->leadindex, ROWS, STATES, StatesInDer);
if (OK != retValue) return retValue; */
/* retValue = getDerWithStateK(pattern->index, pattern->leadindex, der, &numDer, 0);
if (OK != retValue) return retValue; */
/* End of transformation */
#ifdef D
FILE* fid=NULL;
fid = fopen("log_qss.txt","w");
#endif
/* *********************************************************************************** */
/***** Start main simulation loop *****/
while(solverInfo->currentTime < simInfo->stopTime)
{
modelica_integer success = 0;
threadData->currentErrorStage = ERROR_SIMULATION;
omc_alloc_interface.collect_a_little();
#if !defined(OMC_EMCC)
/* try */
MMC_TRY_INTERNAL(simulationJumpBuffer)
{
#endif
#ifdef USE_DEBUG_TRACE
if (useStream[LOG_TRACE])
printf("TRACE: push loop step=%u, time=%.12g\n", currStepNo, solverInfo->currentTime);
#endif
#ifdef D
fprintf(fid,"t = %.8f\n",solverInfo->currentTime);
fprintf(fid,"%16s\t%16s\t%16s\t%16s\t%16s\t%16s\n","tx","x","dx","tq","q","tqp");
for (i = 0; i < STATES; i++)
{
fprintf(fid,"%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\n",tx[i],xik[i],derXik[i],tq[i],qik[i],tqp[i]);
}
#endif
currStepNo++;
ind = minStep(tqp, STATES);
if (isnan(tqp[ind]))
{
#ifdef D
fprintf(fid,"Exit caused by #QNAN!\tind=%d",ind);
#endif
return ISNAN;
}
if (isinf(tqp[ind]))
{
/* If all derivatives are zero, the states stay constant and only the
* time propagates till stop->time.
*/
warningStreamPrint(LOG_STDOUT, 0, "All derivatives are zero at time %f!.\n", sData->timeValue);
solverInfo->currentTime = simInfo->stopTime;
sData->timeValue = solverInfo->currentTime;
continue;
}
qik[ind] = nQh[ind];
xik[ind] = qik[ind];
state[ind] = qik[ind];
tx[ind] = tqp[ind];
tq[ind] = tqp[ind];
solverInfo->currentTime = tqp[ind];
#ifdef D
fprintf(fid,"Index: %d\n\n",ind);
#endif
/* the state[ind] will change again in dTnextQ*/
retValue = deltaQ(data, dQ[ind], ind, &dTnextQ, &nextQ, &diffQ);
if (OK != retValue)
return retValue;
tqp[ind] = tq[ind] + dTnextQ;
nQh[ind] = nextQ;
if (0 != strcmp("ia", MMC_STRINGDATA(data->simulationInfo.outputFormat)))
{
communicateStatus("Running", (solverInfo->currentTime-simInfo->startTime)/(simInfo->stopTime-simInfo->startTime));
}
/* get the derivatives depending on state[ind] */
for (i = 0; i < ROWS; i++)
der[i] = -1;
retValue = getDerWithStateK(pattern->index, pattern->leadindex, der, &numDer, ind);
uinteger k = 0, j = 0;
for (k = 0; k < numDer; k++)
{
j = der[k];
if (j != ind)
{
xik[j] = xik[j] + derXik[j] * (solverInfo->currentTime - tx[j]);
state[j] = xik[j];
tx[j] = solverInfo->currentTime;
}
}
/*
* Recalculate all equations which are affected by state[ind].
* Unfortunately all equations will be calculated up to now. And we need to evaluate
* the equations as f(t,q) and not f(t,x). So all states were saved onto a local stack
* and overwritten by q. After evaluating the equations the states are written back.
*/
for (i = 0; i < STATES; i++)
{
xik[i] = state[i]; /* save current state */
state[i] = qik[i]; /* overwrite current state for dx/dt = f(t,q) */
}
/* update continous system */
sData->timeValue = solverInfo->currentTime;
externalInputUpdate(data);
data->callback->input_function(data);
data->callback->functionODE(data);
data->callback->functionAlgebraics(data);
data->callback->output_function(data);
data->callback->function_storeDelayed(data);
for (i = 0; i < STATES; i++)
{
state[i] = xik[i]; /* restore current state */
}
/*
* Get derivatives affected by state[ind] and write back ALL derivatives. After that we have
* states and derivatives for different times tx.
*/
for (k = 0; k < numDer; k++)
{
j = der[k];
derXik[j] = stateDer[j];
}
derXik[ind] = stateDer[ind]; /* not in every case part of the above derivatives */
for (i = 0; i < STATES; i++)
{
stateDer[i] = derXik[i]; /* write back all derivatives */
}
/* recalculate the time of next change only for the affected states */
for (k = 0; k < numDer; k++)
{
j = der[k];
retValue = deltaQ(data, dQ[j], j, &dTnextQ, &nextQ, &diffQ);
if (OK != retValue)
return retValue;
tqp[j] = solverInfo->currentTime + dTnextQ;
nQh[j] = nextQ;
}
/*sData->timeValue = solverInfo->currentTime;*/
solverInfo->laststep = solverInfo->currentTime;
sim_result.emit(&sim_result, data);
/* check if terminate()=true */
if (terminationTerminate)
{
printInfo(stdout, TermInfo);
fputc('\n', stdout);
infoStreamPrint(LOG_STDOUT, 0, "Simulation call terminate() at time %f\nMessage : %s", data->localData[0]->timeValue, TermMsg);
simInfo->stopTime = solverInfo->currentTime;
}
/* terminate for some cases:
* - integrator fails
* - non-linear system failed to solve
* - assert was called
*/
if (retValIntegrator)
{
retValue = -1 + retValIntegrator;
infoStreamPrint(LOG_STDOUT, 0, "model terminate | Integrator failed. | Simulation terminated at time %g", solverInfo->currentTime);
break;
}
else if (check_nonlinear_solutions(data, 0))
{
retValue = -2;
infoStreamPrint(LOG_STDOUT, 0, "model terminate | non-linear system solver failed. | Simulation terminated at time %g", solverInfo->currentTime);
break;
}
else if (check_linear_solutions(data, 0))
{
retValue = -3;
infoStreamPrint(LOG_STDOUT, 0, "model terminate | linear system solver failed. | Simulation terminated at time %g", solverInfo->currentTime);
break;
}
else if (check_mixed_solutions(data, 0))
{
retValue = -4;
infoStreamPrint(LOG_STDOUT, 0, "model terminate | mixed system solver failed. | Simulation terminated at time %g", solverInfo->currentTime);
break;
}
success = 1;
#if !defined(OMC_EMCC)
}
/* catch */
MMC_CATCH_INTERNAL(simulationJumpBuffer)
#endif
if (!success)
{
retValue = -1;
infoStreamPrint(LOG_STDOUT, 0, "model terminate | Simulation terminated by an assert at time: %g", data->localData[0]->timeValue);
break;
}
TRACE_POP /* pop loop */
}
/* End of main loop */
#ifdef D
fprintf(fid,"t = %.8f\n",solverInfo->currentTime);
fprintf(fid,"%16s\t%16s\t%16s\t%16s\t%16s\t%16s\n","tx","x","dx","tq","q","tqp");
for (i = 0; i < STATES; i++)
{
fprintf(fid,"%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\n",tx[i],xik[i],derXik[i],tq[i],qik[i],tqp[i]);
}
fclose(fid);
#endif
/* free memory*/
free(der);
/* for (i = 0; i < ROWS; i++) free(*(StatesInDer + i));
free(StatesInDer);
free(numStatesInDer); */
free(qik);
free(xik);
free(derXik);
free(tq);
free(tx);
free(tqp);
free(nQh);
free(dQ);
/* end - free memory */
TRACE_POP
return retValue;
}
/*!
* create initial guess dasslColorSymJac
* author: Vitalij Ruge
**/
static short initial_guess_ipopt_sim(OptData *optData, SOLVER_INFO* solverInfo, const short o)
{
double *u0;
int i,j,k,l;
modelica_real ***v;
long double tol;
short printGuess, op=1;
const int nx = optData->dim.nx;
const int nu = optData->dim.nu;
const int np = optData->dim.np;
const int nsi = optData->dim.nsi;
const int nReal = optData->dim.nReal;
char *cflags = (char*)omc_flagValue[FLAG_IIF];
DATA* data = optData->data;
threadData_t *threadData = optData->threadData;
SIMULATION_INFO *sInfo = data->simulationInfo;
if(!data->simulationInfo->external_input.active){
externalInputallocate(data);
}
/* Initial DASSL solver */
DASSL_DATA* dasslData = (DASSL_DATA*) malloc(sizeof(DASSL_DATA));
tol = data->simulationInfo->tolerance;
data->simulationInfo->tolerance = fmin(fmax(tol,1e-8),1e-3);
infoStreamPrint(LOG_SOLVER, 0, "Initial Guess: Initializing DASSL");
sInfo->solverMethod = "dassl";
solverInfo->solverMethod = S_DASSL;
dassl_initial(data, threadData, solverInfo, dasslData);
solverInfo->solverMethod = S_OPTIMIZATION;
solverInfo->solverData = dasslData;
u0 = optData->bounds.u0;
v = optData->v;
if(!data->simulationInfo->external_input.active)
for(i = 0; i< nu;++i)
data->simulationInfo->inputVars[i] = u0[i]/*optData->bounds.scalF[i + nx]*/;
printGuess = (short)(ACTIVE_STREAM(LOG_INIT) && !ACTIVE_STREAM(LOG_SOLVER));
if((double)data->simulationInfo->startTime < optData->time.t0){
double t = data->simulationInfo->startTime;
const int nBoolean = data->modelData->nVariablesBoolean;
const int nInteger = data->modelData->nVariablesInteger;
const int nRelations = data->modelData->nRelations;
FILE * pFile = optData->pFile;
fprintf(pFile, "%lf ",(double)t);
for(i = 0; i < nu; ++i){
fprintf(pFile, "%lf ", (float)data->simulationInfo->inputVars[i]);
}
fprintf(pFile, "%s", "\n");
if(1){
printf("\nPreSim");
printf("\n========================================================\n");
printf("\ndone: time[%i] = %g",0,(double)data->simulationInfo->startTime);
}
while(t < optData->time.t0){
externalInputUpdate(data);
smallIntSolverStep(data, threadData, solverInfo, fmin(t += optData->time.dt[0], optData->time.t0));
printf("\ndone: time[%i] = %g",0,(double)data->localData[0]->timeValue);
sim_result.emit(&sim_result,data,threadData);
fprintf(pFile, "%lf ",(double)data->localData[0]->timeValue);
for(i = 0; i < nu; ++i){
fprintf(pFile, "%lf ", (float)data->simulationInfo->inputVars[i]);
}
fprintf(pFile, "%s", "\n");
}
memcpy(optData->v0, data->localData[0]->realVars, nReal*sizeof(modelica_real));
memcpy(optData->i0, data->localData[0]->integerVars, nInteger*sizeof(modelica_integer));
memcpy(optData->b0, data->localData[0]->booleanVars, nBoolean*sizeof(modelica_boolean));
memcpy(optData->i0Pre, data->simulationInfo->integerVarsPre, nInteger*sizeof(modelica_integer));
memcpy(optData->b0Pre, data->simulationInfo->booleanVarsPre, nBoolean*sizeof(modelica_boolean));
memcpy(optData->v0Pre, data->simulationInfo->realVarsPre, nReal*sizeof(modelica_real));
memcpy(optData->rePre, data->simulationInfo->relationsPre, nRelations*sizeof(modelica_boolean));
memcpy(optData->re, data->simulationInfo->relations, nRelations*sizeof(modelica_boolean));
memcpy(optData->storeR, data->simulationInfo->storedRelations, nRelations*sizeof(modelica_boolean));
if(1){
printf("\n--------------------------------------------------------");
printf("\nfinished: PreSim");
printf("\n========================================================\n");
}
}
if(o == 2 && cflags && strcmp(cflags, ""))
op = 2;
if(printGuess ){
printf("\nInitial Guess");
printf("\n========================================================\n");
printf("\ndone: time[%i] = %g",0,(double)optData->time.t0);
}
for(i = 0, k=1; i < nsi; ++i){
for(j = 0; j < np; ++j, ++k){
externalInputUpdate(data);
if(op==1)
smallIntSolverStep(data, threadData, solverInfo, (double)optData->time.t[i][j]);
else{
rotateRingBuffer(data->simulationData, 1, (void**) data->localData);
importStartValues(data, threadData, cflags, (double)optData->time.t[i][j]);
for(l=0; l<nReal; ++l){
data->localData[0]->realVars[l] = data->modelData->realVarsData[l].attribute.start;
}
}
if(printGuess)
printf("\ndone: time[%i] = %g", k, (double)optData->time.t[i][j]);
memcpy(v[i][j], data->localData[0]->realVars, nReal*sizeof(double));
for(l = 0; l < nx; ++l){
if(((double) v[i][j][l] < (double)optData->bounds.vmin[l]*optData->bounds.vnom[l])
|| (double) (v[i][j][l] > (double) optData->bounds.vmax[l]*optData->bounds.vnom[l])){
printf("\n********************************************\n");
warningStreamPrint(LOG_STDOUT, 0, "Initial guess failure at time %g",(double)optData->time.t[i][j]);
warningStreamPrint(LOG_STDOUT, 0, "%g<= (%s=%g) <=%g",
(double)optData->bounds.vmin[l]*optData->bounds.vnom[l],
data->modelData->realVarsData[l].info.name,
(double)v[i][j][l],
(double)optData->bounds.vmax[l]*optData->bounds.vnom[l]);
printf("\n********************************************");
}
}
}
}
if(printGuess){
printf("\n--------------------------------------------------------");
printf("\nfinished: Initial Guess");
printf("\n========================================================\n");
}
dassl_deinitial(solverInfo->solverData);
solverInfo->solverData = (void*)optData;
sInfo->solverMethod = "optimization";
data->simulationInfo->tolerance = tol;
externalInputFree(data);
return op;
}
/*! \fn sym_euler_im_with_step_size_control_step
*
* Function does one implicit euler step
* and calculates step size for the next step
* using the implicit midpoint rule
*
*/
int sym_euler_im_with_step_size_control_step(DATA* data, threadData_t *threadData, SOLVER_INFO* solverInfo)
{
int retVal = 0;
SIMULATION_DATA *sData = (SIMULATION_DATA*)data->localData[0];
SIMULATION_DATA *sDataOld = (SIMULATION_DATA*)data->localData[1];
DATA_SYM_IMP_EULER* userdata = (DATA_SYM_IMP_EULER*)solverInfo->solverData;
double sc, err, a, b, diff;
double Atol = data->simulationInfo->tolerance, Rtol = data->simulationInfo->tolerance;
int i,j;
double fac = 0.9;
double facmax = 3.5;
double facmin = 0.3;
double saveTime = sDataOld->timeValue;
double targetTime = sDataOld->timeValue + solverInfo->currentStepSize;
if (userdata->firstStep || solverInfo->didEventStep == 1)
{
first_step(data, solverInfo);
userdata->radauStepSizeOld = 0;
}
infoStreamPrint(LOG_SOLVER,0, "new step: time=%e", userdata->radauTime);
while (userdata->radauTime < targetTime)
{
do
{
/*** do one step with original step size ***/
infoStreamPrint(LOG_SOLVER,0, "radauStepSize = %e and time = %e", userdata->radauStepSize, userdata->radauTime);
/* update time */
sDataOld->timeValue = userdata->radauTime;
solverInfo->currentTime = userdata->radauTime + userdata->radauStepSize;
sData->timeValue = solverInfo->currentTime;
/* update step size */
data->callback->symEulerUpdate(data, userdata->radauStepSize);
memcpy(sDataOld->realVars, userdata->radauVars, data->modelData->nStates*sizeof(double));
infoStreamPrint(LOG_SOLVER,0, "first system time = %e", sData->timeValue);
/* evaluate function ODE */
externalInputUpdate(data);
data->callback->input_function(data, threadData);
data->callback->functionODE(data,threadData);
/* save values in y05 */
memcpy(userdata->y05, sData->realVars, data->modelData->nStates*sizeof(double));
/* extrapolate values in y1 */
for (i=0; i<data->modelData->nStates; i++)
{
userdata->y1[i] = 2.0 * userdata->y05[i] - userdata->radauVars[i];
}
/*** do another step with original step size ***/
memcpy(sDataOld->realVars, userdata->y05, data->modelData->nStates*sizeof(double));
/* update time */
sDataOld->timeValue = userdata->radauTime + userdata->radauStepSize;
solverInfo->currentTime = userdata->radauTime + 2.0*userdata->radauStepSize;
sData->timeValue = solverInfo->currentTime;
infoStreamPrint(LOG_SOLVER,0, "second system time = %e", sData->timeValue);
/* update step size */
data->callback->symEulerUpdate(data, userdata->radauStepSize);
/* evaluate function ODE */
externalInputUpdate(data);
data->callback->input_function(data, threadData);
data->callback->functionODE(data, threadData);
/* save values in y2 */
memcpy(userdata->y2, sData->realVars, data->modelData->nStates*sizeof(double));
/*** calculate error ***/
for (i=0, err=0.0; i<data->modelData->nStates; i++)
{
sc = Atol + fmax(fabs(userdata->y2[i]),fabs(userdata->y1[i]))*Rtol;
diff = userdata->y2[i]-userdata->y1[i];
err += (diff*diff)/(sc*sc);
}
err /= data->modelData->nStates;
err = sqrt(err);
userdata->stepsDone += 1;
infoStreamPrint(LOG_SOLVER, 0, "err = %e", err);
infoStreamPrint(LOG_SOLVER, 0, "min(facmax, max(facmin, fac*sqrt(1/err))) = %e", fmin(facmax, fmax(facmin, fac*sqrt(1.0/err))));
/* update step size */
userdata->radauStepSizeOld = 2.0 * userdata->radauStepSize;
userdata->radauStepSize *= fmin(facmax, fmax(facmin, fac*sqrt(1.0/err)));
if (isnan(userdata->radauStepSize))
{
userdata->radauStepSize = 1e-6;
}
} while (err > 1.0 );
userdata->radauTimeOld = userdata->radauTime;
userdata->radauTime += userdata->radauStepSizeOld;
memcpy(userdata->radauVarsOld, userdata->radauVars, data->modelData->nStates*sizeof(double));
memcpy(userdata->radauVars, userdata->y2, data->modelData->nStates*sizeof(double));
}
sDataOld->timeValue = saveTime;
solverInfo->currentTime = sDataOld->timeValue + solverInfo->currentStepSize;
sData->timeValue = solverInfo->currentTime;
/* linear interpolation */
for (i=0; i<data->modelData->nStates; i++)
{
sData->realVars[i] = (userdata->radauVars[i] * (sData->timeValue - userdata->radauTimeOld) + userdata->radauVarsOld[i] * (userdata->radauTime - sData->timeValue))/(userdata->radauTime - userdata->radauTimeOld);
}
/* update first derivative */
infoStreamPrint(LOG_SOLVER,0, "Time %e", sData->timeValue);
for(i=0, j=data->modelData->nStates; i<data->modelData->nStates; ++i, ++j)
{
a = 4.0 * (userdata->y2[i] - 2.0 * userdata->y05[i] + userdata->radauVarsOld[i]) / (userdata->radauStepSizeOld * userdata->radauStepSizeOld);
b = 2.0 * (userdata->y2[i] - userdata->y05[i])/userdata->radauStepSizeOld - userdata->radauTime * a;
data->localData[0]->realVars[j] = a * sData->timeValue + b;
}
/* update step size */
data->callback->symEulerUpdate(data, 0);
solverInfo->solverStepSize = userdata->radauStepSizeOld;
infoStreamPrint(LOG_SOLVER,0, "Step done to %f with step size = %e", sData->timeValue, solverInfo->solverStepSize);
return retVal;
}