void Newton::calcJacobian()
{
// Use analytic Jacobian if available
matrix_t A = _algLoop->getSystemMatrix();
if (A.size1() == _dimSys && A.size2() == _dimSys) {
const double* jac = A.data().begin();
std::copy(jac, jac + _dimSys*_dimSys, _jac);
return;
}
// Alternatively apply finite differences
for (int j = 0; j < _dimSys; ++j) {
// Reset variables for every column
std::copy(_y, _y + _dimSys, _yHelp);
double stepsize = 1e-6 * _yNominal[j];
// Finite differences
_yHelp[j] += stepsize;
calcFunction(_yHelp, _fHelp);
// Build Jacobian in Fortran format
for (int i = 0; i < _dimSys; ++i)
_jac[i + j * _dimSys] = (_fHelp[i] - _f[i]) / stepsize;
_yHelp[j] -= stepsize;
}
}
void RTEuler::doRK1()
{
calcFunction(_tCurrent, _z, _f);
for(int i = 0; i < _dimSys; ++i)
_z[i] += _h * _f[i];
}
double GoldenSectionTwo::getMinimum( double val2 )
{
double x, y=dat2.a, Fxy, Fay, Fxa;
nOperand = 1;
do{
x = getMinimumDat( dat1, y );
Fxy = calcFunction( x, y );
if( fabs(Fxy)< Ftol )
goto DONE;
nOperand = 2;
y = getMinimumDat( dat2, x );
Fxy = calcFunction( y, x );
if( fabs(Fxy)< Ftol )
goto DONE;
nOperand = 1; // must be tested??????
Fay = calcFunction( dat1.a, y );
if( Fay*Fxy > 0)
dat1.a = x;
else
dat1.b = x;
Fxa = calcFunction( x, dat2.a );
if( Fxa*Fxy > 0)
dat2.a = y;
else
dat2.b = y;
}
while( (dat1.b-dat1.a) > dat1.Xtol || (dat2.b-dat2.a) > dat2.Xtol );
x = (dat1.a+dat1.b)/2;
y = (dat2.a+dat2.b)/2;
DONE:
nOperand = 1;
minX = x;
minY = y;
return x;
}
void Hybrj::calcJacobian(double *fjac)
{
for(int j=0; j<_dimSys; ++j)
{
// Reset variables for every column
memcpy(_xHelp,_x,_dimSys*sizeof(double));
// Finite difference
double delta =sqrt(UROUND*std::max(1e-5,std::abs(_x[j])));
_xHelp[j] +=delta;
calcFunction(_xHelp,_fHelp);
// Build Jacobian in Fortran format
for(int i=0; i<_dimSys; ++i)
fjac[i+j*_dimSys] = (_fHelp[i] - _f[i]) /delta;
}
}
void Newton::calcJacobian()
{
for(int j=0; j<_dimSys; ++j)
{
// Reset variables for every column
memcpy(_yHelp,_y,_dimSys*sizeof(double));
double stepsize=1.e-6;//+(1.e-6*_yHelp[j]);
// Finitializee difference
_yHelp[j] += stepsize;
calcFunction(_yHelp,_fHelp);
// Build Jacobian in Fortran format
for(int i=0; i<_dimSys; ++i)
_jac[i+j*_dimSys] = (_fHelp[i] - _f[i]) / stepsize;
_yHelp[j] -=stepsize;
}
}
int Ida::calcJacobian(double t, long int N, N_Vector fHelp, N_Vector errorWeight, N_Vector jthCol, double* y, N_Vector fy, DlsMat Jac)
{
try
{
int l,g;
double fnorm, minInc, *f_data, *fHelp_data, *errorWeight_data, h, srur, delta_inv;
f_data = NV_DATA_S(fy);
errorWeight_data = NV_DATA_S(errorWeight);
fHelp_data = NV_DATA_S(fHelp);
//Get relevant info
_idid = IDAGetErrWeights(_idaMem, errorWeight);
if (_idid < 0)
{
_idid = -5;
throw std::invalid_argument("IDA::calcJacobian()");
}
_idid = IDAGetCurrentStep(_idaMem, &h);
if (_idid < 0)
{
_idid = -5;
throw std::invalid_argument("IDA::calcJacobian()");
}
srur = sqrt(UROUND);
fnorm = N_VWrmsNorm(fy, errorWeight);
minInc = (fnorm != 0.0) ?
(1000.0 * abs(h) * UROUND * N * fnorm) : 1.0;
for(int j=0;j<N;j++)
{
_delta[j] = max(srur*abs(y[j]), minInc/errorWeight_data[j]);
}
for(int j=0;j<N;j++)
{
_deltaInv[j] = 1/_delta[j];
}
// Calculation of the jacobian
if (_jacobianANonzeros != 0)
{
for(int color=1; color <= _maxColors; color++)
{
for(int k=0; k < _dimSys; k++)
{
if((_colorOfColumn[k] ) == color)
{
_ysave[k] = y[k];
y[k]+= _delta[k];
}
}
calcFunction(t, y, fHelp_data,fHelp_data);
for (int k = 0; k < _dimSys; k++)
{
if((_colorOfColumn[k]) == color)
{
y[k] = _ysave[k];
int startOfColumn = k * _dimSys;
for (int j = _jacobianALeadindex[k]; j < _jacobianALeadindex[k+1];j++)
{
l = _jacobianAIndex[j];
g = l + startOfColumn;
Jac->data[g] = (fHelp_data[l] - f_data[l]) * _deltaInv[k];
}
}
}
}
}
/*
//Calculation of J without colouring
for (j = 0; j < N; j++)
{
//N_VSetArrayPointer(DENSE_COL(Jac,j), jthCol);
_ysave[j] = y[j];
y[j] += _delta[j];
calcFunction(t, y, fHelp_data);
y[j] = _ysave[j];
delta_inv = 1.0/_delta[j];
N_VLinearSum(delta_inv, fHelp, -delta_inv, fy, jthCol);
for(int i=0; i<_dimSys; ++i)
{
Jac->data[i+j*_dimSys] = NV_Ith_S(jthCol,i);
}
//DENSE_COL(Jac,j) = N_VGetArrayPointer(jthCol);
}
*/
} //workaround until exception can be catch from c- libraries
catch (std::exception& ex)
{
std::string error = ex.what();
cerr << "IDA integration error: " << error;
return 1;
}
return 0;
}
void Ida::IDACore()
{
_idid = IDAReInit(_idaMem, _tCurrent, _CV_y,_CV_yp);
_idid = IDASetStopTime(_idaMem, _tEnd);
_idid = IDASetInitStep(_idaMem, 1e-12);
if (_idid < 0)
throw std::runtime_error("IDA::ReInit");
bool writeEventOutput = (_settings->getGlobalSettings()->getOutputPointType() == OPT_ALL);
bool writeOutput = !(_settings->getGlobalSettings()->getOutputPointType() == OPT_NONE);
while ((_solverStatus & ISolver::CONTINUE) && !_interrupt )
{
_cv_rt = IDASolve(_idaMem, _tEnd, &_tCurrent, _CV_y, _CV_yp, IDA_ONE_STEP);
_idid = IDAGetNumSteps(_idaMem, &_locStps);
if (_idid != IDA_SUCCESS)
throw std::runtime_error("IDAGetNumSteps failed. The ida mem pointer is NULL");
_idid =IDAGetLastStep(_idaMem, &_h);
if (_idid != IDA_SUCCESS)
throw std::runtime_error("IDAGetLastStep failed. The ida mem pointer is NULL");
//Check if there was at least one output-point within the last solver interval
// -> Write output if true
if (writeOutput)
{
writeIDAOutput(_tCurrent, _h, _locStps);
}
#ifdef RUNTIME_PROFILING
MEASURETIME_REGION_DEFINE(idaStepCompletedHandler, "IDAStepCompleted");
if(MeasureTime::getInstance() != NULL)
{
MEASURETIME_START(measuredFunctionStartValues, idaStepCompletedHandler, "IDAStepCompleted");
}
#endif
//set completed step to system and check if terminate was called
if(_continuous_system->stepCompleted(_tCurrent))
_solverStatus = DONE;
#ifdef RUNTIME_PROFILING
if(MeasureTime::getInstance() != NULL)
{
MEASURETIME_END(measuredFunctionStartValues, measuredFunctionEndValues, (*measureTimeFunctionsArray)[5], idaStepCompletedHandler);
}
#endif
// Perform state selection
bool state_selection = stateSelection();
if (state_selection)
_continuous_system->getContinuousStates(_y);
_zeroFound = false;
// Check if step was successful
if (check_flag(&_cv_rt, "IDA", 1))
{
_solverStatus = ISolver::SOLVERERROR;
break;
}
// A root was found
if ((_cv_rt == IDA_ROOT_RETURN) && !isInterrupted())
{
// IDA is setting _tCurrent to the time where the first event occurred
double _abs = fabs(_tLastEvent - _tCurrent);
_zeroFound = true;
if ((_abs < 1e-3) && _event_n == 0)
{
_tLastEvent = _tCurrent;
_event_n++;
}
else if ((_abs < 1e-3) && (_event_n >= 1 && _event_n < 500))
{
_event_n++;
}
else if ((_abs >= 1e-3))
{
//restart event counter
_tLastEvent = _tCurrent;
_event_n = 0;
}
else
throw std::runtime_error("Number of events exceeded in time interval " + to_string(_abs) + " at time " + to_string(_tCurrent));
// IDA has interpolated the states at time 'tCurrent'
_time_system->setTime(_tCurrent);
// To get steep steps in the result file, two value points (P1 and P2) must be added
//
// Y | (P2) X...........
// | :
// | :
// |........X (P1)
// |---------------------------------->
// | ^ t
// _tCurrent
// Write the values of (P1)
if (writeEventOutput)
{
if(_dimAE>0)
{
_continuous_system->evaluateDAE(IContinuous::CONTINUOUS);
}
else
{
_continuous_system->evaluateAll(IContinuous::CONTINUOUS);
}
writeToFile(0, _tCurrent, _h);
}
_idid = IDAGetRootInfo(_idaMem, _zeroSign);
for (int i = 0; i < _dimZeroFunc; i++)
_events[i] = bool(_zeroSign[i]);
if (_mixed_system->handleSystemEvents(_events))
{
// State variables were reinitialized, thus we have to give these values to the ida-solver
// Take care about the memory regions, _z is the same like _CV_y
_continuous_system->getContinuousStates(_y);
if(_dimAE>0)
{
_mixed_system->getAlgebraicDAEVars(_y+_dimStates);
_continuous_system->getRHS(_yp);
}
calcFunction(_tCurrent, NV_DATA_S(_CV_y), NV_DATA_S(_CV_yp),_dae_res);
}
}
if ((_zeroFound || state_selection)&& !isInterrupted())
{
// Write the values of (P2)
if (writeEventOutput)
{
// If we want to write the event-results, we should evaluate the whole system again
if(_dimAE>0)
{
_continuous_system->evaluateDAE(IContinuous::CONTINUOUS);
}
else
{
_continuous_system->evaluateAll(IContinuous::CONTINUOUS);
}
writeToFile(0, _tCurrent, _h);
}
_idid = IDAReInit(_idaMem, _tCurrent, _CV_y,_CV_yp);
if (_idid < 0)
throw std::runtime_error("IDA::ReInit()");
// Der Eventzeitpunkt kann auf der Endzeit liegen (Time-Events). In diesem Fall wird der Solver beendet, da IDA sonst eine interne Warnung schmeißt
if (_tCurrent == _tEnd)
_cv_rt = IDA_TSTOP_RETURN;
}
// Zähler für die Anzahl der ausgegebenen Schritte erhöhen
++_outStps;
_tLastSuccess = _tCurrent;
if (_cv_rt == IDA_TSTOP_RETURN)
{
_time_system->setTime(_tEnd);
_continuous_system->setContinuousStates(NV_DATA_S(_CV_y));
if(_dimAE>0)
{
_mixed_system->setAlgebraicDAEVars(NV_DATA_S(_CV_y)+_dimStates);
_continuous_system->setStateDerivatives(NV_DATA_S(_CV_yp));
_continuous_system->evaluateDAE(IContinuous::CONTINUOUS);
}
else
{
_continuous_system->evaluateAll(IContinuous::CONTINUOUS);
}
if(writeOutput)
writeToFile(0, _tEnd, _h);
_accStps += _locStps;
_solverStatus = DONE;
}
}
}
/// Method of Golden Section 1D and 2D (reimplemented)
/// xstart, xend, xtol, parameter x - start, end, tolerance
/// ftol function tolerance
/// f_proc function to minimize ( f(x)=>0 )
double GoldenSection::getMinimumDat( GoldenSectionData dat, double val2 )
{
if( (dat.b-dat.a) < dat.Xtol)
goto DONE;
dat.Fa = calcFunction( dat.a, val2);
if( fabs(dat.Fa) < Ftol )
{
dat.b = dat.a;
goto DONE;
}
dat.Fb = calcFunction( dat.b, val2);
if( fabs(dat.Fb) < Ftol )
{
dat.a = dat.b;
goto DONE;
}
if( (dat.Fa*dat.Fb) > 0)
Error( "GoldenSection",
"W01PEexec: No result in specified interval! Change interval!");
dat.x1 = dat.a + .382*(dat.b-dat.a);
dat.x2 = dat.a + .618*(dat.b-dat.a);
dat.Fx1 = calcFunction( dat.x1, val2 );
dat.Fx2 = calcFunction( dat.x2, val2 );
do
{
if( fabs( dat.Fx1 ) < Ftol )
{
dat.a = dat.b = dat.x1;
goto DONE;
}
if( fabs( dat.Fx2 ) < Ftol )
{
dat.a = dat.b =dat. x2;
goto DONE;
}
if( fabs( dat.Fx1) > fabs( dat.Fx2) )
{
dat.a = dat.x1;
if( (dat.b-dat.a) < dat.Xtol)
goto DONE;
dat.x1 = dat.x2;
dat.Fx1 = dat.Fx2;
dat.x2 = dat.a + .618*(dat.b-dat.a);
dat.Fx2 = calcFunction( dat.x2, val2);
}
else
{
dat.b = dat.x2;
if( (dat.b-dat.a) < dat.Xtol)
goto DONE;
dat.x2 = dat.x1;
dat.Fx2 = dat.Fx1;
dat.x1 = dat.a + .382*(dat.b-dat.a);
dat.Fx1 = calcFunction( dat.x1, val2);
}
}
while( (dat.b-dat.a) > dat.Xtol );
DONE:
dat.x1 = (dat.a+dat.b)/2;
return dat.x1;
}
void Ida::initialize()
{
_properties = dynamic_cast<ISystemProperties*>(_system);
_continuous_system = dynamic_cast<IContinuous*>(_system);
_event_system = dynamic_cast<IEvent*>(_system);
_mixed_system = dynamic_cast<IMixedSystem*>(_system);
_time_system = dynamic_cast<ITime*>(_system);
IGlobalSettings* global_settings = dynamic_cast<ISolverSettings*>(_idasettings)->getGlobalSettings();
// Kennzeichnung, dass initialize()() (vor der Integration) aufgerufen wurde
_idid = 5000;
_tLastEvent = 0.0;
_event_n = 0;
SolverDefaultImplementation::initialize();
_dimSys = _continuous_system->getDimContinuousStates();
_dimZeroFunc = _event_system->getDimZeroFunc();
if (_dimSys <= 0)
{
_idid = -1;
throw std::invalid_argument("Ida::initialize()");
}
else
{
// Allocate state vectors, stages and temporary arrays
if (_z)
delete[] _z;
if (_zInit)
delete[] _zInit;
if (_zWrite)
delete[] _zWrite;
if (_zeroSign)
delete[] _zeroSign;
if (_absTol)
delete[] _absTol;
if(_delta)
delete [] _delta;
if(_deltaInv)
delete [] _deltaInv;
if(_ysave)
delete [] _ysave;
_z = new double[_dimSys];
_zInit = new double[_dimSys];
_zWrite = new double[_dimSys];
_zeroSign = new int[_dimZeroFunc];
_absTol = new double[_dimSys];
_delta =new double[_dimSys];
_deltaInv =new double[_dimSys];
_ysave =new double[_dimSys];
memset(_z, 0, _dimSys * sizeof(double));
memset(_zInit, 0, _dimSys * sizeof(double));
memset(_ysave, 0, _dimSys * sizeof(double));
// Counter initialisieren
_outStps = 0;
if (_idasettings->getDenseOutput())
{
// Ausgabeschrittweite
_hOut = global_settings->gethOutput();
}
// Allocate memory for the solver
_idaMem = IDACreate();
if (check_flag((void*) _idaMem, "IDACreate", 0))
{
_idid = -5;
throw std::invalid_argument(/*_idid,_tCurrent,*/"Ida::initialize()");
}
//
// Make Ida ready for integration
//
// Set initial values for IDA
_continuous_system->evaluateAll(IContinuous::CONTINUOUS);
_continuous_system->getContinuousStates(_zInit);
memcpy(_z, _zInit, _dimSys * sizeof(double));
// Get nominal values
_continuous_system->getNominalStates(_absTol);
for (int i = 0; i < _dimSys; i++)
_absTol[i] *= dynamic_cast<ISolverSettings*>(_idasettings)->getATol();
_CV_y0 = N_VMake_Serial(_dimSys, _zInit);
_CV_y = N_VMake_Serial(_dimSys, _z);
_CV_yp = N_VNew_Serial(_dimSys);
_CV_yWrite = N_VMake_Serial(_dimSys, _zWrite);
_CV_absTol = N_VMake_Serial(_dimSys, _absTol);
if (check_flag((void*) _CV_y0, "N_VMake_Serial", 0))
{
_idid = -5;
throw std::invalid_argument("Ida::initialize()");
}
calcFunction(_tCurrent, NV_DATA_S(_CV_y0), NV_DATA_S(_CV_yp));
// Initialize Ida (Initial values are required)
_idid = IDAInit(_idaMem, CV_fCallback, _tCurrent, _CV_y0, _CV_yp);
if (_idid < 0)
{
_idid = -5;
throw std::invalid_argument("Ida::initialize()");
}
// Set Tolerances
_idid = IDASVtolerances(_idaMem, dynamic_cast<ISolverSettings*>(_idasettings)->getRTol(), _CV_absTol); // RTOL and ATOL
if (_idid < 0)
throw std::invalid_argument("IDA::initialize()");
// Set the pointer to user-defined data
_idid = IDASetUserData(_idaMem, _data);
if (_idid < 0)
throw std::invalid_argument("IDA::initialize()");
_idid = IDASetInitStep(_idaMem, 1e-6); // INITIAL STEPSIZE
if (_idid < 0)
throw std::invalid_argument("Ida::initialize()");
_idid = IDASetMaxStep(_idaMem, global_settings->getEndTime() / 10.0); // MAXIMUM STEPSIZE
if (_idid < 0)
throw std::invalid_argument("IDA::initialize()");
_idid = IDASetMaxNonlinIters(_idaMem, 5); // Max number of iterations
if (_idid < 0)
throw std::invalid_argument("IDA::initialize()");
_idid = IDASetMaxErrTestFails(_idaMem, 100);
if (_idid < 0)
throw std::invalid_argument("IDA::initialize()");
_idid = IDASetMaxNumSteps(_idaMem, 1e3); // Max Number of steps
if (_idid < 0)
throw std::invalid_argument(/*_idid,_tCurrent,*/"IDA::initialize()");
// Initialize linear solver
_idid = IDADense(_idaMem, _dimSys);
if (_idid < 0)
throw std::invalid_argument("IDA::initialize()");
// Use own jacobian matrix
//_idid = CVDlsSetDenseJacFn(_idaMem, &CV_JCallback);
//if (_idid < 0)
// throw std::invalid_argument("IDA::initialize()");
if (_dimZeroFunc)
{
_idid = IDARootInit(_idaMem, _dimZeroFunc, &CV_ZerofCallback);
memset(_zeroSign, 0, _dimZeroFunc * sizeof(int));
_idid = IDASetRootDirection(_idaMem, _zeroSign);
if (_idid < 0)
throw std::invalid_argument(/*_idid,_tCurrent,*/"IDA::initialize()");
memset(_zeroSign, -1, _dimZeroFunc * sizeof(int));
memset(_zeroVal, -1, _dimZeroFunc * sizeof(int));
}
initializeColoredJac();
_ida_initialized = true;
//
// IDA is ready for integration
//
// BOOST_LOG_SEV(ida_lg::get(), ida_info) << "IDA initialized";
}
}
void Newton::solve()
{
long int
dimRHS = 1, // Dimension of right hand side of linear system (=b)
info = 0; // Retrun-flag of Fortran code
int
totSteps = 0; // Total number of steps taken
double
atol = _newtonSettings->getAtol(),
rtol = _newtonSettings->getRtol();
// If initialize() was not called yet
if (_firstCall)
initialize();
// Get current values and residuals from system
_algLoop->getReal(_y);
_algLoop->evaluate();
_algLoop->getRHS(_f);
// Reset status flag
_iterationStatus = CONTINUE;
while (_iterationStatus == CONTINUE) {
// Check stopping criterion
if (!_algLoop->isLinear()) {
_iterationStatus = DONE;
for (int i = 0; i < _dimSys; ++i) {
if (std::abs(_f[i]) > atol + rtol * std::abs(_y[i])) {
_iterationStatus = CONTINUE;
break;
}
}
}
if (_iterationStatus == CONTINUE) {
if (totSteps < _newtonSettings->getNewtMax()) {
// Determination of Jacobian (Fortran-format)
if (_algLoop->isLinear() && !_algLoop->isLinearTearing()) {
const matrix_t& A = _algLoop->getSystemMatrix();
const double* jac = A.data().begin();
std::copy(jac, jac + _dimSys*_dimSys, _jac);
dgesv_(&_dimSys, &dimRHS, _jac, &_dimSys, _iHelp, _f, &_dimSys, &info);
std::copy(_f, _f + _dimSys, _y);
_algLoop->setReal(_y);
if (info != 0)
throw ModelicaSimulationError(ALGLOOP_SOLVER,
"error solving linear system (dgesv info: " + to_string(info) + ")");
else
_iterationStatus = DONE;
}
else if (_algLoop->isLinearTearing()) {
_algLoop->setReal(_zeroVec);
_algLoop->evaluate();
_algLoop->getRHS(_f);
const matrix_t& A_sparse = _algLoop->getSystemMatrix();
//m_t A_dense(A_sparse);
const double* jac = A_sparse.data().begin();
std::copy(jac, jac + _dimSys*_dimSys, _jac);
dgesv_(&_dimSys, &dimRHS, _jac, &_dimSys, _iHelp, _f, &_dimSys, &info);
for (int i = 0; i < _dimSys; i++)
_y[i] = -_f[i];
_algLoop->setReal(_y);
_algLoop->evaluate();
if (info != 0)
throw ModelicaSimulationError(ALGLOOP_SOLVER,
"error solving linear tearing system (dgesv info: " + to_string(info) + ")");
else
_iterationStatus = DONE;
}
else {
LogSysVec(_algLoop, "y" + to_string(totSteps), _y);
LogSysVec(_algLoop, "f" + to_string(totSteps), _f);
double phi = 0.0; // line search function
for (int i = 0; i < _dimSys; ++i) {
phi += _f[i] * _f[i];
}
calcJacobian();
// Solve linear System
dgesv_(&_dimSys, &dimRHS, _jac, &_dimSys, _iHelp, _f, &_dimSys, &info);
if (info != 0)
throw ModelicaSimulationError(ALGLOOP_SOLVER,
"error solving nonlinear system (iteration: " + to_string(totSteps)
+ ", dgesv info: " + to_string(info) + ")");
// Increase counter
++ totSteps;
// New iterate
double lambda = 1.0; // step size
double alpha = 1e-4; // guard for sufficient decrease
// first find a feasible step
while (true) {
for (int i = 0; i < _dimSys; i++) {
_yHelp[i] = _y[i] - lambda * _f[i];
_yHelp[i] = std::min(_yMax[i], std::max(_yMin[i], _yHelp[i]));
}
// evaluate function
try {
calcFunction(_yHelp, _fHelp);
}
catch (ModelicaSimulationError& ex) {
if (lambda < 1e-10)
throw ex;
// reduce step size
lambda *= 0.5;
continue;
}
break;
}
// check for solution, e.g. if a linear system is treated here
_iterationStatus = DONE;
for (int i = 0; i < _dimSys; i++) {
if (std::abs(_fHelp[i]) > atol + rtol * std::abs(_yHelp[i])) {
_iterationStatus = CONTINUE;
break;
}
}
// second do line search with quadratic approximation of phi(lambda)
// C.T.Kelley: Solving Nonlinear Equations with Newton's Method,
// no 1 in Fundamentals of Algorithms, SIAM 2003. ISBN 0-89871-546-6.
double phiHelp = 0.0;
for (int i = 0; i < _dimSys; i++)
phiHelp += _fHelp[i] * _fHelp[i];
while (_iterationStatus == CONTINUE) {
// test half step that also serves as max bound for step reduction
double lambdaTest = 0.5*lambda;
for (int i = 0; i < _dimSys; i++) {
_yTest[i] = _y[i] - lambdaTest * _f[i];
_yTest[i] = std::min(_yMax[i], std::max(_yMin[i], _yTest[i]));
}
calcFunction(_yTest, _fTest);
double phiTest = 0.0;
for (int i = 0; i < _dimSys; i++)
phiTest += _fTest[i] * _fTest[i];
// check for sufficient decrease of phiHelp
// and no further decrease with phiTest
// otherwise minimize quadratic approximation of phi(lambda)
if (phiHelp > (1.0 - alpha * lambda) * phi ||
phiTest < (1.0 - alpha * lambda) * phiHelp) {
long int n = 3;
long int ipiv[3];
double bx[] = {phi, phiTest, phiHelp};
double A[] = {1.0, 1.0, 1.0,
0.0, lambdaTest, lambda,
0.0, lambdaTest*lambdaTest, lambda*lambda};
dgesv_(&n, &dimRHS, A, &n, ipiv, bx, &n, &info);
lambda = std::max(0.1*lambda, -0.5*bx[1]/bx[2]);
if (!(lambda >= 1e-10))
throw ModelicaSimulationError(ALGLOOP_SOLVER,
"Can't get sufficient decrease of solution");
if (lambda >= lambdaTest) {
// upper bound 0.5*lambda
lambda = lambdaTest;
std::copy(_yTest, _yTest + _dimSys, _yHelp);
std::copy(_fTest, _fTest + _dimSys, _fHelp);
phiHelp = phiTest;
}
else {
for (int i = 0; i < _dimSys; i++) {
_yHelp[i] = _y[i] - lambda * _f[i];
_yHelp[i] = std::min(_yMax[i], std::max(_yMin[i], _yHelp[i]));
}
calcFunction(_yHelp, _fHelp);
phiHelp = 0.0;
for (int i = 0; i < _dimSys; i++) {
phiHelp += _fHelp[i] * _fHelp[i];
}
}
if (Logger::getInstance()->isOutput(LC_NLS, LL_DEBUG)) {
std::stringstream ss;
ss << "Newton: eq" << to_string(_algLoop->getEquationIndex());
ss << ", time " << _algLoop->getSimTime();
ss << ": lambda = " << lambda;
ss << ", phi = " << phi << " --> " << phiHelp;
Logger::write(ss.str(), LC_NLS, LL_DEBUG);
}
}
// check for sufficient decrease
if (phiHelp <= (1.0 - alpha * lambda) * phi)
break;
}
// take iterate
std::copy(_yHelp, _yHelp + _dimSys, _y);
std::copy(_fHelp, _fHelp + _dimSys, _f);
phi = phiHelp;
}
}
else
throw ModelicaSimulationError(ALGLOOP_SOLVER,
"error solving nonlinear system (iteration limit: " + to_string(totSteps) + ")");
}
}
LogSysVec(_algLoop, "y*", _y);
}
void Newton::solve()
{
long int
dimRHS = 1, // Dimension of right hand side of linear system (=b)
irtrn = 0; // Retrun-flag of Fortran code
int
totStps = 0; // Total number of steps
// If initialize() was not called yet
if (_firstCall)
initialize();
// Get initializeial values from system
_algLoop->getReal(_y);
//_algLoop->evaluate(command);
_algLoop->getRHS(_f);
// Reset status flag
_iterationStatus = CONTINUE;
while(_iterationStatus == CONTINUE)
{
_iterationStatus = DONE;
// Check stopping criterion
calcFunction(_y,_f);
if(totStps)
{
for(int i=0; i<_dimSys; ++i)
{
if(fabs(_f[i]) > _newtonSettings->getAtol() +_newtonSettings->getRtol() * ( fabs(_f[i])))
{
_iterationStatus = CONTINUE;
break;
}
}
}
else
_iterationStatus = CONTINUE;
// New right hand side
//calcFunction(_y,_f);
if(_iterationStatus == CONTINUE)
{
if(totStps < _newtonSettings->getNewtMax())
{
// Determination of Jacobian (Fortran-format)
if(_algLoop->isLinear()&&!_algLoop->isLinearTearing())
{
//calcFunction(_yHelp,_fHelp);
const matrix_t& A = _algLoop->getSystemMatrix();
const double* jac = A.data().begin();
memcpy(_jac, jac, _dimSys*_dimSys*sizeof(double));
dgesv_(&_dimSys,&dimRHS,_jac,&_dimSys,_iHelp,_f,&_dimSys,&irtrn);
memcpy(_y,_f,_dimSys*sizeof(double));
_algLoop->setReal(_y);
if(irtrn != 0)
throw ModelicaSimulationError(ALGLOOP_SOLVER,"error solving linear tearing system");
else
_iterationStatus = DONE;
}
else if(_algLoop->isLinearTearing())
{
long int dimRHS = 1; // Dimension of right hand side of linear system (=b)
long int irtrn = 0; // Retrun-flag of Fortran code
_algLoop->setReal(_zeroVec);
_algLoop->evaluate();
_algLoop->getRHS(_f);
/* adaptor_t f_adaptor(_dimSys,_f);
shared_matrix_t b(_dimSys,1,f_adaptor);*/
//print_m (b, "b vector");
const matrix_t& A_sparse = _algLoop->getSystemMatrix();
//m_t A_dense(A_sparse);
const double* jac = A_sparse.data().begin();
/*double* jac = new double[dimSys*dimSys];
for(int i=0;i<dimSys;i++)
for(int j=0;j<dimSys;j++)
jac[i*_dimSys+j] = A_sparse(i,j);*/
memcpy(_jac, jac, _dimSys*_dimSys*sizeof(double));
dgesv_(&_dimSys, &dimRHS, _jac, &_dimSys, _iHelp, _f,&_dimSys,&irtrn);
// std::vector< int > ipiv (_dimSys); // pivot vector
//lapack::gesv (A_sparse, ipiv,b); // solving the system, b contains x
for(int i=0; i<_dimSys; i++)
_f[i]=-_f[i];
memcpy(_y, _f, _dimSys*sizeof(double));
_algLoop->setReal(_y);
_algLoop->evaluate();
if(irtrn != 0)
throw ModelicaSimulationError(ALGLOOP_SOLVER,"error solving linear tearing system");
else
_iterationStatus = DONE;
}
else
{
calcJacobian();
// Solve linear System
dgesv_(&_dimSys,&dimRHS,_jac,&_dimSys,_iHelp,_f,&_dimSys,&irtrn);
if(irtrn!=0)
{
// TODO: Throw an error message here.
_iterationStatus = SOLVERERROR;
break;
}
// Increase counter
++ totStps;
// New solution
for(int i=0; i<_dimSys; ++i)
_y[i] -= _newtonSettings->getDelta() * _f[i];
}
}
else
_iterationStatus = SOLVERERROR;
}
}
}
