public override double Solve(ImplicitMatrix A, double[] x, double[] b, out int out_StepsPerformed)
{
ExplicitMatrix B = A.AsExplicitMatrix();
int n = A.GetN();
ClearVectorWithValue(x, 0d);
int steps2 = 0;
InitializeOrClear(n);
for (; steps2 < m_SolverSteps; steps2++)
{
for (int i = 0; i < n; i++)
{
double sigma = 0;
for (int j = 0; j < n; j++)
{
if (j != i)
{
sigma = sigma + B.GetValue(i, j) * x[j];
}
}
xnext[i] = (b[i] - sigma) / B.GetValue(i, i);
}
double d = 0;
A.MatrixTimesVector(xnext, r);
for (int i = 0; i < n; i++)
{
x[i] = xnext[i];
d += (r[i] - b[i]) * (r[i] - b[i]);
}
if (d < m_SolverEpsilon)
{
break;
}
}
out_StepsPerformed = steps2;
return(0);
}
// Solve Ax = b for a symmetric, positive definite matrix A
// A is represented implicitly by the function "matVecMult"
// which performs a matrix vector multiple Av and places result in x
// "n" is the length of the vectors x and b
// "epsilon" is the error tolerance
// "steps", as passed, is the maximum number of steps, or 0 (implying MAX_STEPS)
// Upon completion, "steps" contains the number of iterations taken
public override double Solve(ImplicitMatrix A, double[] x, double[] b, out int out_StepsPerformed)
{
int n = A.GetN();
int i, iMax;
double alpha, beta, rSqrLen, rSqrLenOld, u;
InitializeOrClear(n);
if (x.Length != b.Length)
{
throw new System.Exception("Sizes do not match");
}
else if (x.Length != A.GetN())
{
throw new System.Exception("Sizes do not match!" + x.Length + " " + A.GetN());
}
else if (x.Length != n)
{
throw new System.Exception("Sizes do not match!");
}
vecAssign(n, x, b); //x:=b
vecAssign(n, r, b); //r:=b
A.MatrixTimesVector(x, temp); // temp := Ab
vecDiffEqual(n, r, temp); //r := r-temp = b- Ab
rSqrLen = vecSqrLen(n, r);
vecAssign(n, d, r);//d := r = b-Ab
i = 0;
if (m_SolverSteps > 0 && !float.IsInfinity(m_SolverSteps))
{
iMax = m_SolverSteps;
}
else
{
iMax = MAX_STEPS;
}
if (rSqrLen > m_SolverEpsilon)
{
if (logging)
{
Debug.Log("start - eps = " + m_SolverEpsilon + ", iter = " + m_SolverSteps + " A = ");
A.printX();
Debug.Log("b = " + VectorToString(b));
}
while (i < iMax)
{
i++;
A.MatrixTimesVector(d, t);
u = vecDot(n, d, t);
if (double.IsInfinity(u))
{
throw new System.Exception("u = infinity");
}
if (System.Math.Abs(u) <= double.Epsilon)
{
//Debug.Log("(SolveConjGrad) d'Ad = 0\n");
break;
}
// How far should we go?
alpha = rSqrLen / u;
// Take a step along direction d
vecAssign(n, temp, d); //temp = d
vecTimesScalar(n, temp, alpha); //temp = alpha * temp = alpha* d
vecAddEqual(n, x, temp);
ValidateVector(x);
//Debug.Log (toString (temp));
//Debug.Log (u);
//Debug.Log (toString(d));
//Debug.Log (toString(t));
// if (i % 64 != 0)
{
vecAssign(n, temp, t);
vecTimesScalar(n, temp, alpha);
vecDiffEqual(n, r, temp);
}
/*
* else
* {
*
* // For stability, correct r every 64th iteration
* vecAssign(n, r, b);
* A.MatrixTimesVector(x, temp);
* vecDiffEqual(n, r, temp);
* }
*/
rSqrLenOld = rSqrLen;
rSqrLen = vecSqrLen(n, r);
// Converged! Let's get out of here
if (rSqrLen <= m_SolverEpsilon)
{
break;
}
// Change direction: d = r + beta * d
beta = rSqrLen / rSqrLenOld;
vecTimesScalar(n, d, beta);
vecAddEqual(n, d, r);
}
if (logging)
{
Debug.Log("Residual sq: " + rSqrLen);
Debug.Log("Result: x =" + VectorToString(x) + " steps: " + i);
}
}
out_StepsPerformed = i;
return(rSqrLen);
}
public override double Solve(ImplicitMatrix A, double[] x, double[] b, out int out_StepsPerformed)
{
// Reset previous state first.
iter = 0;
rho = 0d;
rho_old = 0d;
rlbl = 0;
int n = A.GetN();
InitializeOrClear(n);
int job_next = cg_rc(n, b, x, r, z, p, q, 1);
int iterations = 1;
while (true)
{
switch (job_next)
{
case 1:
// compute Q = A * P;
ClearVectorWithValue(q, 0);
A.MatrixTimesVector(p, q);
break;
case 2:
// solve M*Z=R, where M is the preconditioning matrix;
for (int i = 0; i < n; ++i)
{
z[i] = r[i]; // choose M = identity matrix
}
break;
case 3:
// compute R = R - A * X;
ClearVectorWithValue(temp, 0);
A.MatrixTimesVector(x, temp);
for (int i = 0; i < n; ++i)
{
r[i] = r[i] - temp[i];
}
break;
case 4:
// check the residual R for convergence.
// If satisfactory, terminate the iteration.
// If too many iterations were taken, terminate the iteration.
double rSqrLen = vecSqrLen(n, r);
// Converged! Let's get out of here
if (rSqrLen <= m_SolverEpsilon || iterations >= m_SolverSteps)
{
out_StepsPerformed = iterations;
if (logging)
{
Debug.Log(VectorToString(x));
Debug.Log(VectorToString(b));
}
return(rSqrLen);
}
break;
}
job_next = cg_rc(n, b, x, r, z, p, q, 2);
++iterations;
}
}