/*!
* eval model DAE
* author: Vitalij Ruge
**/
int refreshSimData(double *x, double *u, double t, IPOPT_DATA_ *iData)
{
int i,j,k;
DATA* data = iData->data;
SIMULATION_DATA *sData = (SIMULATION_DATA*)data->localData[0];
MODEL_DATA *mData = &(data->modelData);
SIMULATION_INFO *sInfo = &(data->simulationInfo);
for(j = 0; j<iData->nx;++j)
{
sData->realVars[j] = x[j]*iData->vnom[j];
}
for(i = 0, k = iData->index_u; i<iData->nu;++i,++j,++k)
sData->realVars[k] = u[i]*iData->vnom[j];
sData->timeValue = t;
/* updateContinuousSystem(iData->data); */
functionODE(data);
return 0;
}
int functionODE_residual(double *t, double *x, double *xd, double *delta,
fortran_integer *ires, double *rpar, fortran_integer *ipar)
{
double timeBackup;
double* statesBackup;
timeBackup = localData->timeValue;
statesBackup = localData->states;
localData->timeValue = *t;
localData->states = x;
functionODE();
/* get the difference between the temp_xd(=localData->statesDerivatives)
and xd(=statesDerivativesBackup) */
for (int i=0; i < localData->nStates; i++) {
delta[i] = localData->statesDerivatives[i] - xd[i];
}
localData->states = statesBackup;
localData->timeValue = timeBackup;
return 0;
}
/******************************* interpolation module ************************************************/
int
interpolation_control(const int dideventstep, double interpolationStep,
double fixStep, double stop) {
int i,l;
if(sim_verbose >= LOG_SOLVER)
{
fprintf(stdout, "| info | dense output: $$$$$\t interpolate data at %g\n", interpolationStep); fflush(NULL);
}
/* if(sim_verbose >= LOG_SOLVER)
* {
* cout << "oldTime,Time,interpolate data at " << globalData->oldTime << ", "
* << globalData->timeValue << ", " << interpolationStep << endl; fflush(NULL);
* } /* for debugging */
if(dideventstep == 1)
{
/* Emit data after an event */
sim_result.emit(&sim_result,data);
}
if(((interpolationStep > globalData->oldTime) && (interpolationStep < globalData->timeValue)) ||
((dideventstep == 1) && (interpolationStep < globalData->timeValue)))
{
double** k = work_states;
double backupTime = globalData->timeValue;
double backupTime_old = globalData->oldTime;
double* backupstats = (double*) malloc(globalData->nStates * sizeof(double));
double* backupderivatives = (double*) malloc(globalData->nStates * sizeof(double));
double* backupstats_old = (double*) malloc(globalData->nStates * sizeof(double));
double bstar[9];
double numerator = 0, sigma, sh, sum;
/* save states and derivatives as they're altered by linear interpolation method */
for(i = 0; i < globalData->nStates; i++)
{
backupstats[i] = globalData->states[i];
backupderivatives[i] = globalData->statesDerivatives[i];
backupstats_old[i] = globalData->states_old[i];
}
do
{
if(!(backupTime == backupTime_old)) /* don't interpolate during an event */
{
/* calculate dense output interpolation parameter sigma */
sh = interpolationStep - globalData->timeValue;
sigma = sh / globalData->current_stepsize;
for(i = 0; i < dop5dense_s; i++)
{
/* compute bstar vector components using Horner's scheme */
numerator = dop_bst[i][4] +
sigma * (dop_bst[i][3] +
sigma * (dop_bst[i][2] +
sigma * (dop_bst[i][1] +
sigma * dop_bst[i][0])));
bstar[i] = numerator / dop_bst[i][5];
}
for(i = 0; i < globalData->nStates; i++)
{
sum = 0;
for(l = 0; l < dop5dense_s; l++)
{
sum = sum + bstar[l] * k[l][i];
}
globalData->states[i] = globalData->states[i] + sh * sum;
}
/* set global time value to interpolated time */
globalData->timeValue = interpolationStep;
/* update all dependent variables */
functionODE(NULL);
functionAlgebraics(NULL);
saveZeroCrossings();
/* Emit interpolated data at the current time step */
sim_result.emit(&sim_result,data);
}
interpolationStep = interpolationStep + fixStep;
} while ((interpolationStep <= stop + fixStep) && (interpolationStep < backupTime));
/* update old data */
globalData->oldTime = backupTime;
/* reset data for next solver step */
globalData->timeValue = backupTime;
for(i = 0; i < globalData->nStates; i++)
{
globalData->states[i] = backupstats[i];
globalData->statesDerivatives[i] = backupderivatives[i];
}
free(backupstats);
free(backupderivatives);
free(backupstats_old);
} else {
globalData->oldTime = globalData->timeValue;
}
return 0;
}
