Foam::scalarSquareMatrix Foam::scalarSquareMatrix::LUinvert() const
{
scalarSquareMatrix luMatrix = *this;
scalarSquareMatrix luInvert(luMatrix.n());
scalarField column(luMatrix.n());
labelList pivotIndices(luMatrix.n());
LUDecompose(luMatrix, pivotIndices);
for (label j = 0; j < luMatrix.n(); j++)
{
for (label i = 0; i < luMatrix.n(); i++)
{
column[i] = 0.0;
}
column[j] = 1.0;
LUBacksubstitute(luMatrix, pivotIndices, column);
for (label i = 0; i < luMatrix.n(); i++)
{
luInvert[i][j] = column[i];
}
}
return luInvert;
}
Foam::scalar Foam::rodas34::solve
(
const scalar x0,
const scalarField& y0,
const scalarField& dydx0,
const scalar dx,
scalarField& y
) const
{
odes_.jacobian(x0, y0, dfdx_, dfdy_);
for (register label i=0; i<n_; i++)
{
for (register label j=0; j<n_; j++)
{
a_[i][j] = -dfdy_[i][j];
}
a_[i][i] += 1.0/(gamma*dx);
}
LUDecompose(a_, pivotIndices_);
// Calculate k1:
forAll(k1_, i)
{
k1_[i] = dydx0[i] + dx*d1*dfdx_[i];
}
Foam::LUscalarMatrix::LUscalarMatrix(const Matrix<scalar>& matrix)
:
scalarMatrix(matrix),
pivotIndices_(n())
{
LUDecompose(*this, pivotIndices_);
}
Foam::LUscalarMatrix::LUscalarMatrix(const scalarSquareMatrix& matrix)
:
scalarSquareMatrix(matrix),
pivotIndices_(n())
{
LUDecompose(*this, pivotIndices_);
}
Foam::scalar Foam::Rosenbrock23::solve
(
const scalar x0,
const scalarField& y0,
const scalarField& dydx0,
const scalar dx,
scalarField& y
) const
{
odes_.jacobian(x0, y0, dfdx_, dfdy_);
for (label i=0; i<n_; i++)
{
for (label j=0; j<n_; j++)
{
a_(i, j) = -dfdy_(i, j);
}
a_(i, i) += 1.0/(gamma*dx);
}
LUDecompose(a_, pivotIndices_);
// Calculate k1:
forAll(k1_, i)
{
k1_[i] = dydx0[i] + dx*d1*dfdx_[i];
}
Foam::scalar Foam::det(const SymmetricSquareMatrix<Type>& matrix)
{
SymmetricSquareMatrix<Type> matrixTmp = matrix;
LUDecompose(matrixTmp);
return detDecomposed(matrixTmp);
}
Foam::scalar Foam::det(SquareMatrix<Type>& matrix)
{
labelList pivotIndices(matrix.n());
label sign;
LUDecompose(matrix, pivotIndices, sign);
return detDecomposed(matrix, sign);
}
Foam::LUscalarMatrix::LUscalarMatrix(const scalarSquareMatrix& matrix)
:
scalarSquareMatrix(matrix),
comm_(Pstream::worldComm),
pivotIndices_(m())
{
LUDecompose(*this, pivotIndices_);
}
void Foam::LUDecompose
(
scalarSquareMatrix& matrix,
labelList& pivotIndices
)
{
label sign;
LUDecompose(matrix, pivotIndices, sign);
}
Foam::scalar Foam::det(const SquareMatrix<Type>& matrix)
{
SquareMatrix<Type> matrixTmp = matrix;
labelList pivotIndices(matrix.n());
label sign;
LUDecompose(matrixTmp, pivotIndices, sign);
return detDecomposed(matrixTmp, sign);
}
float FloatMatrix::det()
{
LUDecompose();
float res = d;
for(unsigned long int i = 0; i<rows();i++)
{
res*=(*this)(i,i);
}
return res;
}
void Foam::LUsolve
(
scalarSquareMatrix& matrix,
Field<Type>& sourceSol
)
{
labelList pivotIndices(matrix.n());
LUDecompose(matrix, pivotIndices);
LUBacksubstitute(matrix, pivotIndices, sourceSol);
}
Foam::SymmetricSquareMatrix<Type> Foam::inv
(
const SymmetricSquareMatrix<Type>& matrix
)
{
SymmetricSquareMatrix<Type> matrixTmp(matrix);
LUDecompose(matrixTmp);
return invDecomposed(matrixTmp);
}
FloatMatrix FloatMatrix::inverse()
{
vector<float> col(cols(),0.0);
FloatMatrix inv(rows(),cols());
LUDecompose();
for(unsigned long int j = 0; j < rows(); j++)
{
for(unsigned long int i =0; i < rows(); i++)
{
col[i] = 0.0;
}
col[j] = 1.0;
LUBackSubstitute(col);
for(unsigned long int k = 0; k< rows(); k++)
{
inv(k,j) = col[k];
}
}
return inv;
}
/*----------------------------------------------------------------------------------------------------------------------
| Returns the LU decomposition of this SquareMatrix.
*/
SquareMatrix * SquareMatrix::LUDecomposition() const
{
PHYCAS_ASSERT(dim > 0);
SquareMatrix * L = new SquareMatrix(*this);
double * scaling = new double[dim];
int * permutation = new int[dim];
double ** a = L->GetMatrixAsRawPointer();
int err_code = LUDecompose(a, dim, scaling, permutation, NULL);
if (err_code == 0)
{
// LUDecompose worked
delete [] scaling;
delete [] permutation;
return L;
}
// Should throw an exception here
delete L;
delete [] scaling;
delete [] permutation;
return 0;
}
int InvertMatrix (double **a, int n, double *col, int *indx, double **a_inv)
/* **a = matrix represented as vector of row pointers */
/* n = order of matrix */
/* *col = work vector of size n */
/* *indx = work vector of size n */
/* **a_inv = inverse of input matrix a (matrix a is destroyed) */
{
int rc, i, j;
rc = LUDecompose(a, n, col, indx, (double *)NULL);
if (rc == FALSE)
{
for (j = 0; j < n; j++)
{
for (i = 0; i < n; i++)
col[i] = 0.0;
col[j] = 1.0;
LUBackSubst(a, n, indx, col);
for (i = 0; i < n; i++)
a_inv[i][j] = col[i];
}
}
return rc;
}
bool Foam::seulex::seul
(
const scalar x0,
const scalarField& y0,
const scalar dxTot,
const label k,
scalarField& y,
const scalarField& scale
) const
{
label nSteps = nSeq_[k];
scalar dx = dxTot/nSteps;
for (label i=0; i<n_; i++)
{
for (label j=0; j<n_; j++)
{
a_[i][j] = -dfdy_[i][j];
}
a_[i][i] += 1.0/dx;
}
LUDecompose(a_, pivotIndices_);
scalar xnew = x0 + dx;
odes_.derivatives(xnew, y0, dy_);
LUBacksubstitute(a_, pivotIndices_, dy_);
yTemp_ = y0;
for (label nn=1; nn<nSteps; nn++)
{
yTemp_ += dy_;
xnew += dx;
if (nn == 1 && k<=1)
{
scalar dy1 = 0.0;
for (label i=0; i<n_; i++)
{
dy1 += sqr(dy_[i]/scale[i]);
}
dy1 = sqrt(dy1);
odes_.derivatives(x0 + dx, yTemp_, dydx_);
for (label i=0; i<n_; i++)
{
dy_[i] = dydx_[i] - dy_[i]/dx;
}
LUBacksubstitute(a_, pivotIndices_, dy_);
scalar dy2 = 0.0;
for (label i=0; i<n_; i++)
{
dy2 += sqr(dy_[i]/scale[i]);
}
dy2 = sqrt(dy2);
theta_ = dy2/min(1.0, dy1 + SMALL);
if (theta_ > 1.0)
{
return false;
}
}
odes_.derivatives(xnew, yTemp_, dy_);
LUBacksubstitute(a_, pivotIndices_, dy_);
}
for (label i=0; i<n_; i++)
{
y[i] = yTemp_[i] + dy_[i];
}
return true;
}
bool Foam::seulex::seul
(
const scalar x0,
const scalarField& y0,
const scalar dxTot,
const label k,
scalarField& y,
const scalarField& scale
) const
{
label nSteps = nSeq_[k];
scalar dx = dxTot/nSteps;
for (label i=0; i<n_; i++)
{
for (label j=0; j<n_; j++)
{
a_(i, j) = -dfdy_(i, j);
}
a_(i, i) += 1/dx;
}
LUDecompose(a_, pivotIndices_);
scalar xnew = x0 + dx;
odes_.derivatives(xnew, y0, dy_);
LUBacksubstitute(a_, pivotIndices_, dy_);
yTemp_ = y0;
for (label nn=1; nn<nSteps; nn++)
{
yTemp_ += dy_;
xnew += dx;
if (nn == 1 && k<=1)
{
scalar dy1 = 0;
for (label i=0; i<n_; i++)
{
dy1 += sqr(dy_[i]/scale[i]);
}
dy1 = sqrt(dy1);
odes_.derivatives(x0 + dx, yTemp_, dydx_);
for (label i=0; i<n_; i++)
{
dy_[i] = dydx_[i] - dy_[i]/dx;
}
LUBacksubstitute(a_, pivotIndices_, dy_);
const scalar denom = min(1, dy1 + SMALL);
scalar dy2 = 0;
for (label i=0; i<n_; i++)
{
// Test of dy_[i] to avoid overflow
if (mag(dy_[i]) > scale[i]*denom)
{
theta_ = 1;
return false;
}
dy2 += sqr(dy_[i]/scale[i]);
}
dy2 = sqrt(dy2);
theta_ = dy2/denom;
if (theta_ > 1)
{
return false;
}
}
odes_.derivatives(xnew, yTemp_, dy_);
LUBacksubstitute(a_, pivotIndices_, dy_);
}
for (label i=0; i<n_; i++)
{
y[i] = yTemp_[i] + dy_[i];
}
return true;
}
void Foam::KRR4::solve
(
const ODE& ode,
scalar& x,
scalarField& y,
scalarField& dydx,
const scalar eps,
const scalarField& yScale,
const scalar hTry,
scalar& hDid,
scalar& hNext
) const
{
scalar xTemp = x;
yTemp_ = y;
dydxTemp_ = dydx;
ode.jacobian(xTemp, yTemp_, dfdx_, dfdy_);
scalar h = hTry;
for (register label jtry=0; jtry<maxtry; jtry++)
{
for (register label i=0; i<n_; i++)
{
for (register label j=0; j<n_; j++)
{
a_[i][j] = -dfdy_[i][j];
}
a_[i][i] += 1.0/(gamma*h);
}
LUDecompose(a_, pivotIndices_);
for (register label i=0; i<n_; i++)
{
g1_[i] = dydxTemp_[i] + h*c1X*dfdx_[i];
}
LUBacksubstitute(a_, pivotIndices_, g1_);
for (register label i=0; i<n_; i++)
{
y[i] = yTemp_[i] + a21*g1_[i];
}
x = xTemp + a2X*h;
ode.derivatives(x, y, dydx_);
for (register label i=0; i<n_; i++)
{
g2_[i] = dydx_[i] + h*c2X*dfdx_[i] + c21*g1_[i]/h;
}
LUBacksubstitute(a_, pivotIndices_, g2_);
for (register label i=0; i<n_; i++)
{
y[i] = yTemp_[i] + a31*g1_[i] + a32*g2_[i];
}
x = xTemp + a3X*h;
ode.derivatives(x, y, dydx_);
for (register label i=0; i<n_; i++)
{
g3_[i] = dydx[i] + h*c3X*dfdx_[i] + (c31*g1_[i] + c32*g2_[i])/h;
}
LUBacksubstitute(a_, pivotIndices_, g3_);
for (register label i=0; i<n_; i++)
{
g4_[i] = dydx_[i] + h*c4X*dfdx_[i]
+ (c41*g1_[i] + c42*g2_[i] + c43*g3_[i])/h;
}
LUBacksubstitute(a_, pivotIndices_, g4_);
for (register label i=0; i<n_; i++)
{
y[i] = yTemp_[i] + b1*g1_[i] + b2*g2_[i] + b3*g3_[i] + b4*g4_[i];
yErr_[i] = e1*g1_[i] + e2*g2_[i] + e3*g3_[i] + e4*g4_[i];
}
x = xTemp + h;
if (x == xTemp)
{
FatalErrorIn("ODES::KRR4")
<< "stepsize not significant"
<< exit(FatalError);
}
scalar maxErr = 0.0;
for (register label i=0; i<n_; i++)
{
maxErr = max(maxErr, mag(yErr_[i]/yScale[i]));
}
maxErr /= eps;
if (maxErr <= 1.0)
{
hDid = h;
hNext = (maxErr > errcon ? safety*h*pow(maxErr, pgrow) : grow*h);
return;
}
else
{
hNext = safety*h*pow(maxErr, pshrink);
h = (h >= 0.0 ? max(hNext, shrink*h) : min(hNext, shrink*h));
}
}
FatalErrorIn("ODES::KRR4")
<< "exceeded maxtry"
<< exit(FatalError);
}
void Foam::SIBS::SIMPR
(
const ODE& ode,
const scalar xStart,
const scalarField& y,
const scalarField& dydx,
const scalarField& dfdx,
const scalarSquareMatrix& dfdy,
const scalar deltaX,
const label nSteps,
scalarField& yEnd
) const
{
scalar h = deltaX/nSteps;
scalarSquareMatrix a(n_);
for (register label i=0; i<n_; i++)
{
for (register label j=0; j<n_; j++)
{
a[i][j] = -h*dfdy[i][j];
}
++a[i][i];
}
labelList pivotIndices(n_);
LUDecompose(a, pivotIndices);
for (register label i=0; i<n_; i++)
{
yEnd[i] = h*(dydx[i] + h*dfdx[i]);
}
LUBacksubstitute(a, pivotIndices, yEnd);
scalarField del(yEnd);
scalarField ytemp(n_);
for (register label i=0; i<n_; i++)
{
ytemp[i] = y[i] + del[i];
}
scalar x = xStart + h;
ode.derivatives(x, ytemp, yEnd);
for (register label nn=2; nn<=nSteps; nn++)
{
for (register label i=0; i<n_; i++)
{
yEnd[i] = h*yEnd[i] - del[i];
}
LUBacksubstitute(a, pivotIndices, yEnd);
for (register label i=0; i<n_; i++)
{
ytemp[i] += (del[i] += 2.0*yEnd[i]);
}
x += h;
ode.derivatives(x, ytemp, yEnd);
}
for (register label i=0; i<n_; i++)
{
yEnd[i] = h*yEnd[i] - del[i];
}
LUBacksubstitute(a, pivotIndices, yEnd);
for (register label i=0; i<n_; i++)
{
yEnd[i] += ytemp[i];
}
}
