void CILDMModifiedMethod::newton_for_timestep(C_INT metabolite_number, C_FLOAT64 & y_consistent, C_INT & info)
{
C_INT i, iter, itermax, flag_newton;
iter = 0;
itermax = 150;
flag_newton = 1; // set flag_newton=0 to print the iteration steps
C_FLOAT64 tol, err;
tol = 1e-6;
err = 10.0;
C_INT dim = mData.dim;
C_FLOAT64 d_y, deriv;
d_y = 0;
deriv = mJacobian_initial(metabolite_number, metabolite_number);
if (deriv == 0)
{
return;
}
info = 0;
C_FLOAT64 number2conc = mpModel->getNumber2QuantityFactor() / mpModel->getCompartments()[0]->getInitialValue();
//C_FLOAT62number2conc = 1.;
//this is an ugly hack that only makes sense if all metabs are in the same compartment
//at the moment is is the only case the algorithm deals with
CVector<C_FLOAT64> y_newton; //current state converted to concentrations
y_newton.resize(dim);
for (i = 0; i < dim; ++i)
y_newton[i] = mY_initial[i] * number2conc;
CVector<C_FLOAT64> dydt;
dydt.resize(dim);
while (err > tol)
{
iter ++;
if (iter > itermax)
{
info = 1;
break;
}
y_newton[metabolite_number] = y_newton[metabolite_number] + d_y;
calculateDerivativesX(y_newton.array(), dydt.array());
d_y = - 1 / deriv * dydt[metabolite_number];
if (err > fabs(d_y))
err = fabs(d_y);
}
y_consistent = y_newton[metabolite_number];
return;
}
void CILDMModifiedMethod::newton_new(C_INT *index_metab, C_INT & slow, C_INT & info)
{
C_INT i, j, k, m, iter, iterations, itermax;
C_INT nrhs, ok, fast, flag_newton;
flag_newton = 1; // set flag_newton=1 to print temporaly steps of newton iterations
C_FLOAT64 tol, err;
C_INT dim = mData.dim;
fast = dim - slow;
CVector<C_INT> ipiv;
ipiv.resize(fast);
CVector<C_FLOAT64> s_22_array;
s_22_array.resize(fast*fast);
CVector<C_FLOAT64> gf_newton;
gf_newton.resize(fast);
CVector<C_FLOAT64> d_yf;
d_yf.resize(dim);
CVector<C_FLOAT64> y_newton;
y_newton.resize(dim);
CVector<C_FLOAT64> yf_newton;
yf_newton.resize(fast);
CVector<C_FLOAT64> dydt_newton;
dydt_newton.resize(dim);
CVector<C_FLOAT64> g_newton;
g_newton.resize(dim);
CMatrix<C_FLOAT64> Jac_fast;
Jac_fast.resize(fast, fast);
C_FLOAT64 g1, g2 = 0.0;
mY_cons.resize(dim);
nrhs = 1;
tol = 1e-6;
err = 10.0;
iter = 0;
itermax = 150;
iterations = 0;
info = 0;
C_FLOAT64 number2conc = mpModel->getNumber2QuantityFactor() / mpModel->getCompartments()[0]->getInitialValue();
//C_FLOAT64 number2conc = 1.;
for (i = 0; i < fast; i++)
{
m = index_metab[i];
for (j = 0; j < fast; j++)
{
k = index_metab[ j];
if ((m > -1) & (k > -1))
Jac_fast(i, j) = mJacobian_initial(m, k);
else
{
info = 3;
return;
}
}
}
for (i = 0; i < dim; i++)
y_newton[i] = mY_initial[i] * number2conc;
for (i = 0; i < fast; i++)
for (j = 0; j < fast; j++)
s_22_array[j + fast*i] = Jac_fast(j, i);
for (i = 0; i < dim; i++)
d_yf[i] = 0.;
while (err > tol)
{
iter ++;
if (iter > itermax)
{
info = 1;
return;
}
for (i = 0; i < dim; i++)
y_newton[i] = y_newton[i] + d_yf[i];
calculateDerivativesX(y_newton.array(), dydt_newton.array());
for (i = 0; i < fast; i++)
{
j = index_metab[i];
gf_newton[i] = -1. * dydt_newton[j];
}
/* int dgesv_(integer *n, integer *nrhs, doublereal *a, integer
* *lda, integer *ipiv, doublereal *b, integer *ldb, integer *info)
*
* -- LAPACK driver routine (version 3.0) --
* Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
* Courant Institute, Argonne National Lab, and Rice University
* March 31, 1993
*
*
* Purpose
* =======
*
* DGESV computes the solution to a real system of linear equations
* A * X = B,
* where A is an N-by-N matrix and X and B are N-by-NRHS matrices.
*
* The LU decomposition with partial pivoting and row interchanges is
* used to factor A as
* A = P * L * U,
* where P is a permutation matrix, L is unit lower triangular, and U is
* upper triangular. The factored form of A is then used to solve the
* system of equations A * X = B.
*
* Arguments
* =========
*
* N (input) INTEGER
* The number of linear equations, i.e., the order of the
* matrix A. N >= 0.
*
* NRHS (input) INTEGER
* The number of right hand sides, i.e., the number of columns
* of the matrix B. NRHS >= 0.
*
* A (input/output) DOUBLE PRECISION array, dimension (LDA,N)
* On entry, the N-by-N coefficient matrix A.
* On exit, the factors L and U from the factorization
* A = P*L*U; the unit diagonal elements of L are not stored.
*
* LDA (input) INTEGER
* The leading dimension of the array A. LDA >= max(1,N).
*
* IPIV (output) INTEGER array, dimension (N)
* The pivot indices that define the permutation matrix P;
* row i of the matrix was interchanged with row IPIV(i).
*
* B (input/output) DOUBLE PRECISION array, dimension (LDB,NRHS)
* On entry, the N-by-NRHS matrix of right hand side matrix B.
* On exit, if INFO = 0, the N-by-NRHS solution matrix X.
*
* LDB (input) INTEGER
* The leading dimension of the array B. LDB >= max(1,N).
*
*
* INFO (output) INTEGER
* = 0: successful exit
* < 0: if INFO = -i, the i-th argument had an illegal value
* > 0: if INFO = i, U(i,i) is exactly zero. The factorization
* has been completed, but the factor U is exactly
* singular, so the solution could not be computed.
*/
dgesv_(&fast, &nrhs, s_22_array.array(), &fast, ipiv.array(), gf_newton.array(), &fast, &ok);
if (ok != 0)
{
info = 2;
return;
}
for (i = 0; i < fast; i++)
d_yf[i] = 0;
for (j = 0; j < fast; j++)
{
k = index_metab[j];
for (i = 0; i < dim; i ++)
{
if (i == k)
d_yf[k] = gf_newton[j];
}
}
err = -10.;
for (i = 0; i < fast; i++)
{
gf_newton[i] = fabs(gf_newton[i]);
if (err < gf_newton[i])
err = gf_newton[i];
}
iterations = iterations + 1;
// stop criterion of newton method
// C_FLOAT64 g1, g2 = 0.0;
// g2 = err;
if (iter == 1)
g1 = 3.0 * err;
else
g1 = g2;
g2 = err;
if (g2 / g1 > 1.0)
{
info = 1;
return;
}
} /* end while */
for (i = 0; i < dim; i++)
mY_cons[i] = y_newton[i];
info = 0;
return;
}
/**
Deuflhard Iteration: Prove Deuflhard criteria, find consistent initial value for DAE
output: info - if Deuflhard is satisfied
*/
void CILDMModifiedMethod::deuflhard_metab(C_INT & slow, C_INT & info)
{
C_INT i, j, info_newton;
C_INT dim = mData.dim;
C_INT fast = dim - slow;
C_INT flag_deufl;
flag_deufl = 1; // set flag_deufl=0 to print temporaly steps for this function
C_FLOAT64 max = 0;
CVector<C_FLOAT64> re;
CVector<C_FLOAT64> dxdt_relax;
CVector<C_FLOAT64> x_relax;
CVector<C_FLOAT64> x_help;
CVector<C_FLOAT64> dxdt;
CVector<C_FLOAT64> help;
help.resize(dim);
CVector<C_INT> index;
index.resize(dim);
CVector<C_INT> index_temp;
index_temp.resize(dim);
C_FLOAT64 eps;
eps = 1 / fabs(mR(dim - fast , dim - fast));
//eps = fabs(mR(dim - fast - 1, dim - fast - 1)) / fabs(mR(dim - fast , dim - fast));
mat_anal_fast_space(slow);
for (i = 0; i < dim; i++)
{
index[i] = i;
index_temp[i] = i;
}
for (i = 0; i < dim; i++)
{
help[i] = mVfast_space[i];
}
evalsort(help.array(), index.array(), dim);
for (i = 0; i < dim; i++)
index_temp[i] = index[i];
for (i = 0; i < dim; i++)
{
index[i] = index_temp[dim - i - 1];
}
C_FLOAT64 number2conc = mpModel->getNumber2QuantityFactor() / mpModel->getCompartments()[0]->getInitialValue();
//C_FLOAT64 number2conc = 1.;
dxdt.resize(dim);
for (j = 0; j < dim; j++)
dxdt[j] = 0.;
//CVector<C_FLOAT64> x_help;
x_help.resize(dim);
for (j = 0; j < dim; j++)
{
x_help[j] = mY_initial[j] * number2conc;
}
// mpModel->calculateDerivativesX(dxdt.array());
calculateDerivativesX(x_help.array(), dxdt.array());
info_newton = 0;
newton_new(index.array(), slow, info_newton);
if (info_newton)
{
// TODO
info = 1;
return;
}
x_relax.resize(dim);
for (i = 0; i < dim; i++)
x_relax[i] = mY_cons[i];
//CVector<C_FLOAT64> dxdt_relax;
dxdt_relax.resize(dim);
calculateDerivativesX(x_relax.array(), dxdt_relax.array());
//CVector<C_FLOAT64> re;
re.resize(dim);
// stop criterion for slow reaction modes
for (i = 0; i < dim; i++)
{
re[i] = fabs(dxdt_relax[i] - dxdt[i]);
re[i] = re[i] * eps;
for (j = 0; j < fast; j ++)
if (i == index[j])
re[i] = 0;
}
for (i = 0; i < dim; i++)
if (max < re[i])
max = re[i];
if (max >= mDtol)
info = 1;
else
info = 0;
return;
}
void CILDMModifiedMethod::step(const double & deltaT)
{
C_INT dim = mData.dim;
C_INT fast = 0;
C_INT slow = dim - fast;
C_INT slow2, fast2;
slow2 = dim;
fast2 = dim - slow2;
C_INT i, j;
mY_initial.resize(dim);
mJacobian_initial.resize(dim, dim);
mQ.resize(dim, dim);
mR.resize(dim, dim);
mTd.resize(dim, dim);
mTdInverse.resize(dim, dim);
mQz.resize(dim, dim);
mTd_save.resize(dim, dim);
mTdInverse_save.resize(dim, dim);
mpModel->updateSimulatedValues(mReducedModel);
// TO REMOVE : mpModel->applyAssignments();
mpModel->calculateJacobianX(mJacobian, 1e-6, 1e-12);
C_INT flag_jacob;
flag_jacob = 1; // Set flag_jacob=0 to print Jacobian
C_FLOAT64 number2conc = mpModel->getNumber2QuantityFactor() / mpModel->getCompartments()[0]->getInitialValue();
//C_FLOAT64 number2conc = 1.;
//this is an ugly hack that only makes sense if all metabs are in the same compartment
//at the moment is is the only case the algorithm deals with
CVector<C_FLOAT64> Xconc; //current state converted to concentrations
Xconc.resize(dim);
for (i = 0; i < dim; ++i)
Xconc[i] = mY[i] * number2conc;
for (i = 0; i < dim; i++)
mY_initial[i] = mY[i];
CVector<C_FLOAT64> Xconc_initial; //current state converted to concentrations
Xconc_initial.resize(dim);
for (i = 0; i < dim; ++i)
Xconc_initial[i] = mY_initial[i] * number2conc;
// save initial Jacobian before next time step
for (i = 0; i < dim; i++)
for (j = 0; j < dim; j++)
mJacobian_initial(i, j) = mJacobian(i, j);
// Next time step
integrationStep(deltaT);
mpModel->updateSimulatedValues(mReducedModel);
// TO REMOVE : mpModel->applyAssignments();
// Calculate Jacobian for time step control
mpModel->calculateJacobianX(mJacobian, 1e-6, 1e-12);
//CMatrix<C_FLOAT64> mTd_save;
for (i = 0; i < dim; i++)
for (j = 0; j < dim; j++)
{
mTd_save(i, j) = 0;
mTdInverse_save(i, j) = 0;
}
for (i = 0; i < dim; i++)
for (j = 0; j < dim; j++)
{
mTd(i, j) = 0;
mTdInverse(i, j) = 0;
}
/** Schur Decomposition of Jacobian (reordered).
Output: mQ - transformation matrix mR - block upper triangular matrix (with ordered eigenvalues) */
C_INT failed = 0;
C_INT info_schur = 0;
C_INT number, k;
C_INT failed_while = 0;
C_INT flag_deufl;
flag_deufl = 1;
C_FLOAT64 max = 0.;
//C_FLOAT64 max = 0;
CVector<C_FLOAT64> re;
CVector<C_FLOAT64> dxdt_relax;
CVector<C_FLOAT64> x_relax;
CVector<C_FLOAT64> x_help;
CVector<C_FLOAT64> dxdt;
CVector<C_FLOAT64> x_zero;
CVector<C_FLOAT64> dxdt_zero;
CVector<C_FLOAT64> dxdt_real;
CVector<C_FLOAT64> help;
CVector<C_INT> index;
CVector<C_INT> index_temp;
CMatrix<C_FLOAT64> orthog_prove;
orthog_prove.resize(dim, dim);
C_INT info;
CVector<C_INT> index_metab;
index_metab.resize(dim);
/** Schur transformation of Jacobian */
schur(info_schur);
if (info_schur)
{
CCopasiMessage(CCopasiMessage::WARNING,
MCTSSAMethod + 9, mTime - deltaT);
goto integration;
}
for (i = 0; i < dim; i++)
for (j = 0; j < dim; j++)
mTdInverse(i, j) = mQ(j, i);
C_INT flag_schur;
flag_schur = 1; // set flag_schur = 0 to print Schur decomposition of jacobian (matrices mR and transformation mQ)
mY_cons.resize(dim); // consistent initial vector for DAE
/* If complex eigenvalues */
//BUG 873
if (mR(dim - 1, dim - 1) == mR(dim - 2 , dim - 2))
if (dim == 2)
{
slow = dim;
goto integration;
}
// If positive eigenvalues
if (mR(dim - 1, dim - 1) >= 0)
{
slow = dim;
fast = 0;
CCopasiMessage(CCopasiMessage::WARNING,
MCTSSAMethod + 10, mTime - deltaT);
failed = 1;
goto integration;
}
// Iterations to determine the number of slow metabolites
while ((slow2 > 1))
{
slow2 = slow2 - 1;
fast2 = dim - slow2;
if (mR(dim - fast2, dim - fast2) >= 0)
{
failed = 1;
goto integration;
}
deuflhard_metab(slow2, info);
if (info)
{
failed_while = 1;
goto integration;
}
}
//end of iterations to determine the number of slow metabolites
/** end of the block %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
/** %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
integration:
slow = slow2;
fast = fast2;
if ((failed == 1) || (failed_while == 1))
{
if (slow < dim)
{
fast = fast - 1;
slow = dim - fast;
if ((fast >= 1) && (mR(slow - 1, slow - 1) == mR(slow , slow)))
fast = fast - 1;
slow = dim - fast;
}
}
mSlow = slow;
if (slow == dim)
CCopasiMessage(CCopasiMessage::WARNING,
MCTSSAMethod + 11, mTime);
for (i = 0; i < dim; i++)
for (j = 0; j < dim; j++)
// mTdInverse(i,j) = mQ(i,j);
mTd(i, j) = mQ(i, j);
// Flag for print Tabs
mat_anal_mod(slow);
mat_anal_metab(slow);
mat_anal_mod_space(slow);
mat_anal_fast_space(slow);
// This block proves which metabolite could be considered as QSS. In development
/** Begin of the block %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
C_INT flag_dev;
flag_dev = 1;
if (flag_dev == 0)
{
C_INT temp;
temp = dim - 1;
// Mode Analysis to find the dominant metabolites in the modes
mat_anal_mod(temp);
//index_metab = -1, if there is no dominant metabolites in corresponding mode,
//and is equal
// to the number of dominant metabolite in opposite case
// Dominant metabolite - its contribution to the mode is larger as 70%
for (j = 0; j < dim; j++)
index_metab[j] = -1;
for (i = 0; i < dim ; i ++)
for (j = 0; j < dim; j++)
if (mVslow(dim - i - 1, j) > 70)
index_metab[i] = j;
C_FLOAT64 y_cons;
info = 0;
k = 0;
number = index_metab[k];
if (number > - 1)
newton_for_timestep(number, y_cons, info);
while (k < dim - 1)
{
if (number > -1)
{
dxdt.resize(dim);
for (j = 0; j < dim; j++)
dxdt[j] = 0.;
//CVector<C_FLOAT64> x_help;
x_help.resize(dim);
for (j = 0; j < dim; j++)
{
x_help[j] = mY_initial[j] * number2conc;
}
calculateDerivativesX(x_help.array(), dxdt.array());
info = 0;
//NEWTON: Looking for consistent initial value for DAE system
//Output: y_cons, info
newton_for_timestep(number, y_cons, info);
if (info)
{
// TODO info: newton iteration stop
}
if (info == 0)
{
// CVector<C_FLOAT64> x_relax;
x_relax.resize(dim);
for (i = 0; i < dim; i ++)
if (i == number)
x_relax[i] = y_cons;
else
x_relax[i] = x_help[i];
//CVector<C_FLOAT64> dxdt_relax;
dxdt_relax.resize(dim);
calculateDerivativesX(x_relax.array(), dxdt_relax.array());
//CVector<C_FLOAT64> re;
re.resize(dim);
C_FLOAT64 eps;
eps = 1 / fabs(mR(dim - k - 1 , dim - k - 1));
// stop criterion for slow reaction modes
for (i = 0; i < dim; i++)
{
if (i == number)
re[i] = 0;
else
{
re[i] = fabs(dxdt_relax[i] - dxdt[i]);
re[i] = re[i] * eps;
}
}
//C_FLOAT64 max = 0.;
for (i = 0; i < dim; i++)
if (max < re[i])
max = re[i];
if (max >= mDtol)
info = 1;
else
info = 0;
}
}
k = k + 1;
number = index_metab[k];
max = 0;
}
}
/** end of the of block %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
/** %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
mpModel->updateSimulatedValues(mReducedModel);
// TO REMOVE : mpModel->applyAssignments();
// Calculate Jacobian for time step control
mpModel->calculateJacobianX(mJacobian, 1e-6, 1e-12);
// new entry for every entry contains the current data of currently step
setVectors(slow);
// set the stepcounter
mCurrentStep += 1;
return;
}
/**
Deuflhard Iteration: Prove Deuflhard criteria, find consistent initial value for DAE
output: info - if Deuflhard is satisfied for given slow; transformation matrices
mTd and mTdinverse
*/
void CILDMMethod::deuflhard(C_INT & slow, C_INT & info)
{
C_INT i, j;
C_INT dim = mData.dim;
C_INT fast = dim - slow;
C_INT flag_deufl;
flag_deufl = 1; // set flag_deufl = 0 to print the results of calculations
/* calculations before relaxing yf to slow manifold */
CVector<C_FLOAT64> c_full;
c_full.resize(dim);
CVector<C_FLOAT64> c_slow;
c_slow.resize(slow);
/* the vector mY is the current state of the system*/
C_FLOAT64 number2conc = 1.; // mpModel->getNumber2QuantityFactor()
// / mpModel->getCompartments()[0]->getInitialValue();
//this is an ugly hack that only makes sense if all metabs are in the same compartment
//at the moment is is the only case the algorithm deals with
CVector<C_FLOAT64> Xconc; //current state converted to concentrations
Xconc.resize(dim);
for (i = 0; i < dim; ++i)
Xconc[i] = mY_initial[i] * number2conc;
for (i = 0; i < dim; i++)
{
c_full[i] = 0.0;
for (j = 0; j < dim; j++)
c_full[i] = c_full[i] + mTdInverse(i, j) * Xconc[j];
}
for (j = 0; j < slow; j++)
c_slow[j] = c_full[j];
for (j = 0; j < fast; j++)
mCfast[j] = c_full[j + slow];
CVector<C_FLOAT64> g_full;
g_full.resize(dim);
CVector<C_FLOAT64> g_slow;
g_slow.resize(slow);
CVector<C_FLOAT64> g_fast;
g_fast.resize(fast);
CVector<C_FLOAT64> dxdt;
dxdt.resize(dim);
mpModel->updateSimulatedValues(mReducedModel);
for (j = 0; j < dim; j++)
dxdt[j] = 0.;
CVector<C_FLOAT64> x_help;
x_help.resize(dim);
for (j = 0; j < dim; j++)
{
x_help[j] = mY_initial[j] * number2conc;
}
calculateDerivativesX(x_help.array(), dxdt.array());
for (i = 0; i < dim; i++)
{
g_full[i] = 0.0;
for (j = 0; j < dim; j++)
g_full[i] = g_full[i] + mTdInverse(i, j) * dxdt[j];
}
for (j = 0; j < slow; j++)
g_slow[j] = g_full[j];
info = 0;
/** NEWTON: Looking for consistent initial value for DAE system
Output: mCfast, info */
newton(c_slow.array(), slow, info);
if (info)
{
/* TODO */
return;
}
/* calculation of g_relax at point x_relax (after relaxing yf to slow manifold)*/
CVector<C_FLOAT64> c_relax;
c_relax.resize(dim);
CVector<C_FLOAT64> x_relax;
x_relax.resize(dim);
CVector<C_FLOAT64> dxdt_relax;
dxdt_relax.resize(dim);
CVector<C_FLOAT64> g_relax;
g_relax.resize(dim);
for (i = 0; i < slow; i++)
c_relax[i] = c_slow[i];
for (i = slow; i < dim; i++)
c_relax[i] = mCfast[i - slow];
for (i = 0; i < dim; i++)
{
x_relax[i] = 0.0;
for (j = 0; j < dim; j++)
x_relax[i] = x_relax[i] + mTd(i, j) * c_relax[j];
}
calculateDerivativesX(x_relax.array(), dxdt_relax.array());
for (i = 0; i < dim; i++)
{
g_relax[i] = 0.0;
for (j = 0; j < dim; j++)
g_relax[i] = g_relax[i] + mTdInverse(i, j) * dxdt_relax[j];
}
CVector<C_FLOAT64> re;
re.resize(slow);
/* stop criterion for slow reaction modes */
for (i = 0; i < slow; i++)
{
re[i] = fabs(g_relax[i] - g_slow[i]);
re[i] = re[i] * mEPS;
}
C_FLOAT64 max = 0.;
for (i = 0; i < slow; i++)
if (max < re[i])
max = re[i];
C_FLOAT64 max1;
C_FLOAT64 norm = 0;
for (i = 0; i < slow; i++)
norm = norm + fabs(g_relax[i] - g_slow[i]);
max1 = norm * mEPS;
if (max >= mDtol / mpModel->getNumber2QuantityFactor())
info = 1;
else
info = 0;
return;
}
void CILDMMethod::newton(C_FLOAT64 *ys, C_INT & slow, C_INT & info)
{
C_INT i, j, iter, iterations, itermax;
C_INT nrhs, ok, fast;
C_FLOAT64 tol, err;
C_INT dim = mData.dim;
fast = dim - slow;
CVector<C_INT> ipiv;
ipiv.resize(fast);
CVector<C_FLOAT64> s_22_array;
s_22_array.resize(fast*fast);
CVector<C_FLOAT64> gf_newton;
gf_newton.resize(fast);
CVector<C_FLOAT64> d_yf;
d_yf.resize(fast);
CVector<C_FLOAT64> y_newton;
y_newton.resize(dim);
CVector<C_FLOAT64> yf_newton;
yf_newton.resize(fast);
CVector<C_FLOAT64> x_newton;
x_newton.resize(dim);
CVector<C_FLOAT64> dxdt_newton;
dxdt_newton.resize(dim);
CVector<C_FLOAT64> g_newton;
g_newton.resize(dim);
CMatrix<C_FLOAT64> S_22;
S_22.resize(fast, fast);
C_FLOAT64 g1, g2 = 0;
nrhs = 1;
//tol = 1e-9;
tol = 1e-9 / mpModel->getNumber2QuantityFactor();
err = 10.0 / mpModel->getNumber2QuantityFactor();
iter = 0;
itermax = 100;
iterations = 0;
info = 0;
for (i = 0; i < fast; i++)
for (j = 0; j < fast; j++)
S_22(i, j) = mQz(i, j);
for (i = 0; i < fast; i++)
yf_newton[i] = mCfast[i];
for (i = 0; i < fast; i++)
for (j = 0; j < fast; j++)
s_22_array[j + fast*i] = S_22(j, i);
for (i = 0; i < fast; i++)
d_yf[i] = 0.;
while (err > tol)
{
iter ++;
if (iter > itermax)
{
info = 1;
return;
}
for (i = 0; i < fast; i++)
yf_newton[i] = yf_newton[i] + d_yf[i];
/* back transformation */
for (i = 0; i < slow; i++)
y_newton[i] = ys[i];
for (i = slow; i < dim; i++)
y_newton[i] = yf_newton[i - slow];
for (i = 0; i < dim; i++)
{
x_newton[i] = 0.0;
for (j = 0; j < dim; j++)
x_newton[i] = x_newton[i] + mTd(i, j) * y_newton[j];
}
calculateDerivativesX(x_newton.array(), dxdt_newton.array());
for (i = 0; i < dim; i++)
{
g_newton[i] = 0.;
for (j = 0; j < dim; j++)
g_newton[i] = g_newton[i] + mTdInverse(i, j) * dxdt_newton[j];
}
// for (i = 0; i < fast; i++)
// gf_newton[i] = -1. * g_newton[i + slow];
for (i = 0; i < fast; i++)
{
gf_newton[i] = -1. * g_newton[i + slow];
}
/* int dgesv_(integer *n, integer *nrhs, doublereal *a, integer
* *lda, integer *ipiv, doublereal *b, integer *ldb, integer *info)
*
* -- LAPACK driver routine (version 3.0) --
* Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
* Courant Institute, Argonne National Lab, and Rice University
* March 31, 1993
*
*
* Purpose
* =======
*
* DGESV computes the solution to a real system of linear equations
* A * X = B,
* where A is an N-by-N matrix and X and B are N-by-NRHS matrices.
*
* The LU decomposition with partial pivoting and row interchanges is
* used to factor A as
* A = P * L * U,
* where P is a permutation matrix, L is unit lower triangular, and U is
* upper triangular. The factored form of A is then used to solve the
* system of equations A * X = B.
*
* Arguments
* =========
*
* N (input) INTEGER
* The number of linear equations, i.e., the order of the
* matrix A. N >= 0.
*
* NRHS (input) INTEGER
* The number of right hand sides, i.e., the number of columns
* of the matrix B. NRHS >= 0.
*
* A (input/output) DOUBLE PRECISION array, dimension (LDA,N)
* On entry, the N-by-N coefficient matrix A.
* On exit, the factors L and U from the factorization
* A = P*L*U; the unit diagonal elements of L are not stored.
*
* LDA (input) INTEGER
* The leading dimension of the array A. LDA >= max(1,N).
*
* IPIV (output) INTEGER array, dimension (N)
* The pivot indices that define the permutation matrix P;
* row i of the matrix was interchanged with row IPIV(i).
*
* B (input/output) DOUBLE PRECISION array, dimension (LDB,NRHS)
* On entry, the N-by-NRHS matrix of right hand side matrix B.
* On exit, if INFO = 0, the N-by-NRHS solution matrix X.
*
* LDB (input) INTEGER
* The leading dimension of the array B. LDB >= max(1,N).
*
*
* INFO (output) INTEGER
* = 0: successful exit
* < 0: if INFO = -i, the i-th argument had an illegal value
* > 0: if INFO = i, U(i,i) is exactly zero. The factorization
* has been completed, but the factor U is exactly
* singular, so the solution could not be computed.
*/
dgesv_(&fast, &nrhs, s_22_array.array(), &fast, ipiv.array(), gf_newton.array(), &fast, &ok);
if (ok != 0)
{
info = 2;
break;
}
for (i = 0; i < fast; i++)
d_yf[i] = gf_newton[i];
err = -10.;
/* for (i = 0; i < fast; i++)
{
gf_newton[i] = fabs(gf_newton[i]);
if (err < gf_newton[i])
err = gf_newton[i];
}
*/
for (i = 0; i < fast; i++)
{
gf_newton[i] = fabs(gf_newton[i]);
if (err < gf_newton[i])
err = gf_newton[i];
}
iterations = iterations + 1;
/* stop criterion of newton method */
// C_FLOAT64 g1, g2;
// g2 = err;
if (iter == 1)
g1 = 3.0 * err;
else
g1 = g2;
g2 = err;
if (g2 / g1 > 1.0)
{
info = 1;
break;
}
} /* end while */
for (i = 0; i < fast; i++)
mCfast[i] = yf_newton[i];
return;
}
