/// <summary>
/// Gets column with number coumnnum from matrix res
/// </summary>
/// <param name="columnNum">Column number (zero based)</param>
/// <returns>Vector containing copy of column's elements</returns>
public Vector CloneColumn(int columnNum)
{
if (0 > columnNum || columnNum > ColumnDimension - 1)
{
throw new IndexOutOfRangeException("Column index is out of range");
}
Vector v = Vector.Zeros(RowDimension);
for (int i = 0; i < RowDimension; i++)
{
v[i] = a[i][columnNum];
}
return(v);
}
/// <summary>Matrix inverse for a lower triangular matrix</summary>
/// <param name="L"></param>
/// <returns></returns>
public Matrix InverseLower()
{
int n = this.ColumnDimension;
var I = Matrix.Identity(n, n);
var invLtr = new double[n][];
for (int col = 0; col < n; col++)
{
Vector x = Vector.Zeros(n);
x[col] = 1;
invLtr[col] = this.SolveLower(x);
}
var invL = new Matrix(invLtr).Transpose();
return(invL);
}
/// <summary>
/// Returns a vector whose elements are the absolute values of the given vector elements
/// </summary>
/// <param name="v">Vector to operate with</param>
/// <returns>Vector v1 such that for each i = 0...dim(v) v1[i] = |v[i]|</returns>
public Vector Abs()
{
if (v == null)
{
return(new Vector());
}
int n = v.Length;
Vector y = Vector.Zeros(n);
for (int i = 0; i < n; i++)
{
y[i] = Math.Abs(v[i]);
}
return(y);
}
/// <summary>Performs element-wise division of two vectors</summary>
/// <param name="a">Numerator vector</param>
/// <param name="b">Denominator vector</param>
/// <returns>Result of scalar multiplication</returns>
public static Vector operator /(Vector a, Vector b)
{
if (a.Length != b.Length)
{
throw new InvalidOperationException("Cannot element-wise divide vectors of different length");
}
double[] res = Vector.Zeros(a.Length);
for (int i = 0; i < a.Length; i++)
{
if (b[i] == 0.0d)
{
throw new DivideByZeroException("Cannot divide by zero");
}
res[i] = a[i] / b[i];
}
return(res);
}
/// <summary>Matrix multiplication, y = v * A</summary>
/// <param name="v">Vector</param>
/// <returns>y</returns>
public Vector timesRight(Vector v)
{
double[] vv = v;
Vector result = Vector.Zeros(n);
unchecked // Turns off integral overflow checking: small speedup
{
for (int i = 0; i < n; i++)
{
if (indices[i] != null)
{
for (int k = 0; k < count[i]; k++)
{
result[indices[i][k]] += vv[i] * items[i][k];
}
}
}
}
return(result);
}
/// <summary>Forward substitution routine for solving Lx = b, where L is a lower-triangular matrix</summary>
/// <param name="b"></param>
/// <returns></returns>
public Vector SolveLower(Vector b)
{
Vector x = Vector.Zeros(m);
for (int i = 0; i < m; i++)
{
x[i] = b[i];
var idx = indices[i];
var its = items[i];
for (int k = 0; k < count[i]; k++)
{
int j = idx[k];
if (j < i)
{
x[i] -= its[k] * x[j];
}
}
x[i] /= this[i][i];
}
return(x);
}
/// <summary>Matrix multiplication</summary>
/// <param name="v">Vector</param>
/// <returns></returns>
public Vector times(Vector v)
{
double[] vv = v;
Vector result = Vector.Zeros(m);
unchecked // Turns off integral overflow checking: small speedup
{
for (int i = 0; i < m; i++)
{
if (indices[i] != null)
{
double s = 0;
for (int k = 0; k < count[i]; k++)
{
s += items[i][k] * vv[indices[i][k]];
}
result[i] = s;
}
}
}
return(result);
}
/// <summary>Implementation of Runge-Kutta algoritm with per-point accurancy control from
/// Dormand and Prince article.
/// (J.R.Dormand, P.J.Prince, A family of embedded Runge-Kuttae formulae)</summary>
/// <param name="t0">Left end of current time span</param>
/// <param name="x0">Initial phase vector value</param>
/// <param name="f">System right parts vector function</param>
/// <param name="opts">Options used by solver</param>
/// <example>Let our problem will be:
/// dx/dt=y+1,
/// dy/dt=-x+2.
/// x(0)=0, y(0)=1.
/// To solve it, we just have to write
/// <code>
/// var sol=Ode.RK547M(0,new Vector(0,1),(t,x)=>new Vector(y+1,-x+2));
/// </code>
/// and then enumerate solution point from <see cref="System.IEnumerable"/> 'sol'.
/// </example>
/// <returns>Endless sequence of solution points</returns>
public static IEnumerable <SolPoint> RK547M(double t0, Vector x0, Func <double, Vector, Vector> f, Options opts)
{
// Safety factors for automatic step control
// See Solving Ordinary Differential Equations I Non stiff problems by E. Hairer, S.P. Norsett, G.Wanner
// 2nd edition, Springer.
// Safety factor is recommended on page 168
const double SafetyFactor = 0.8;
const double MaxFactor = 5.0d; // Maximum possible step increase
const double MinFactor = 0.2d; // Maximum possible step decrease
int Refine = 4;
Vector S = Vector.Zeros(Refine - 1);
for (int i = 0; i < Refine - 1; i++)
{
S[i] = (double)(i + 1) / Refine;
}
double t = t0;
Vector x = x0.Clone(); // Lower order approximation
Vector x1 = x0.Clone(); // Higher order approximation
int n = x0.Length; // Dimensions of the system
// Initial step value. During computational process, we modify it.
double dt = opts.InitialStep;
// Create formulae parameters(a's,b's and c's)
// In original article, a is a step array.
double[][] a = new double[6][];
a[0] = new double[] { 1 / 5d };
a[1] = new double[] { 3 / 40d, 9 / 40d };
a[2] = new double[] { 44 / 45d, -56 / 15d, 32 / 9d };
a[3] = new double[] { 19372 / 6561d, -25360 / 2187d, 64448 / 6561d, -212 / 729d };
a[4] = new double[] { 9017 / 3168d, -355 / 33d, 46732 / 5247d, 49 / 176d, -5103 / 18656d };
a[5] = new double[] { 35 / 384d, 0, 500 / 1113d, 125 / 192d, -2187 / 6784d, 11 / 84d };
Vector c = new Vector(0, 1 / 5d, 3 / 10d, 4 / 5d, 8 / 9d, 1, 1);
// Coeffs for higher order
Vector b1 = new Vector(5179 / 57600d, 0, 7571 / 16695d, 393 / 640d, -92097 / 339200d, 187 / 2100d, 1 / 40d);
// Coeffs for lower order
Vector b = new Vector(35 / 384d, 0, 500 / 1113d, 125 / 192d, -2187 / 6784d, 11 / 84d, 0);
const int s = 7; // == c.Length
double dt0;
// Compute initial step (see E. Hairer book)
if (opts.InitialStep == 0)
{
double d0 = 0.0d, d1 = 0.0d;
double[] sc = new double[n];
var f0 = f(t0, x0);
for (int i = 0; i < n; i++)
{
sc[i] = opts.AbsoluteTolerance + opts.RelativeTolerance * Math.Abs(x0[i]);
d0 = Math.Max(d0, Math.Abs(x0[i]) / sc[i]);
d1 = Math.Max(d1, Math.Abs(f0[i]) / sc[i]);
}
var h0 = Math.Min(d0, d1) < 1e-5 ? 1e-6 : 1e-2 * (d0 / d1);
var f1 = f(t0 + h0, x0 + h0 * f0);
double d2 = 0;
for (int i = 0; i < n; i++)
{
d2 = Math.Max(d2, Math.Abs(f0[i] - f1[i]) / sc[i] / h0);
}
dt = Math.Max(d1, d2) <= 1e-15 ? Math.Max(1e-6, h0 * 1e-3) : Math.Pow(1e-2 / Math.Max(d1, d2), 1 / 5d);
if (dt > 100 * h0)
{
dt = 100 * h0;
}
}
else
{
dt = opts.InitialStep;
}
dt0 = dt;
// Output initial point
double tout = t0;
Vector xout = x0.Clone();
if (opts.OutputStep > 0) // Store previous solution point if OutputStep is specified (non-zero)
{
tout += opts.OutputStep;
}
yield return(new SolPoint(t0, x0.Clone()));
// Pre-allocate arrays
Vector[] k = new Vector[s];
Vector[] x2 = new Vector[s - 1];
for (int i = 0; i < s - 1; i++)
{
x2[i] = Vector.Zeros(n);
}
Vector prevX = Vector.Zeros(n);
double prevErr = 1.0d; // Previous error, used for step PI-filtering
double prevDt;
while (true) // Main loop - produces numerical solution
{
Vector.Copy(x1, prevX);
double e = 0.0d; // error factor, should be < 1
//int ii = 0;
do
{
prevDt = dt;
Vector.Copy(prevX, x1);
// Compute internal method variables.
k[0] = dt * f(t, x1);
for (int i = 1; i < s; i++)
{
Vector.Copy(x1, x2[i - 1]);
for (int j = 0; j < i; j++)
{
x2[i - 1].MulAdd(k[j], a[i - 1][j]);
}
k[i] = dt * f(t + dt * c[i], x2[i - 1]);
}
//Try to compute X in the next time point.
Vector.Copy(prevX, x);
for (int l = 0; l < s; l++)
{
x.MulAdd(k[l], b[l]);
x1.MulAdd(k[l], b1[l]);
}
// Compute error (see p. 168 of book indicated above)
// error compulation in L-infinity norm is commented
e = Math.Abs(x[0] - x1[0]) / Math.Max(opts.AbsoluteTolerance, opts.RelativeTolerance * Math.Max(Math.Abs(prevX[0]), Math.Abs(x1[0])));
for (int i = 1; i < n; i++)
{
e = Math.Max(e, Math.Abs(x[i] - x1[i]) / Math.Max(opts.AbsoluteTolerance, opts.RelativeTolerance * Math.Max(Math.Abs(prevX[i]), Math.Abs(x1[i]))));
}
// PI-filter. Beta = 0.08
dt = e == 0 ? dt : dt *Math.Min(MaxFactor, Math.Max(MinFactor, SafetyFactor *Math.Pow(1.0d / e, 1.0d / 5.0d) * Math.Pow(prevErr, 0.08d)));
if (opts.MaxStep < Double.MaxValue)
{
dt = Math.Min(dt, opts.MaxStep);
}
//if (opts.MinStep > 0) dt = Math.Max(dt, opts.MinStep);
if (Double.IsNaN(dt))
{
throw new ArgumentException("Derivatives function returned NaN");
}
if (dt < 1e-12)
{
throw new ArgumentException("Cannot generate numerical solution");
}
} while(e > 1.0d); // Repeat until solving vector euclidean norm doesn't satisfy break condition
prevErr = e;
// Output data
if (opts.OutputStep > 0) // Output points with specified step
{
while (t <= tout && tout <= t + prevDt)
{
yield return(new SolPoint(tout, Vector.Lerp(tout, t, xout, t + prevDt, x1)));
tout += opts.OutputStep;
}
}
else
if (Refine > 1) // Output interpolated points set by Refine property
{
Vector ts = Vector.Zeros(S.Length);
for (int i = 0; i < S.Length; i++)
{
ts[i] = t + prevDt * S[i];
}
var ys = RKinterp(S, xout, k);
for (int i = 0; i < S.Length; i++)
{
yield return(new SolPoint(ts[i], ys[i]));
}
}
yield return(new SolPoint(t + prevDt, x1.Clone()));
// Update current time and state
t = t + prevDt;
Vector.Copy(x1, xout);
}
}
/// <summary>
/// Execute predictor-corrector scheme for Nordsieck's method
/// </summary>
/// <param name="qcurr">current method order</param>
/// <param name="x">system phase vector to compute</param>
/// <param name="e0">initial error vector (en(0) in LSODE)</param>
/// <param name="t">current time</param>
/// <param name="xprev">initial value of phase vector</param>
/// <param name="z0">Zn(0) in LSODE - initial History matrix value</param>
/// <param name="dt">current step size</param>
/// <param name="l">current Nordsieck's parameters vector</param>
/// <param name="b">current b0 in Gear's scheme</param>
/// <param name="tau">current step change poarameter tau(q,q)</param>
/// <param name="zn">current Z Nordsieck's matrix to change</param>
/// <param name="f">right parts vector</param>
/// <param name="opts">current options</param>
/// <returns>en - current error vector</returns>
private static NordsieckState PredictorCorrectorScheme(NordsieckState currstate, ref bool flag, Func <double, Vector, Vector> f, Options opts)
{
int n = currstate.xn.Length;
NordsieckState newstate = new NordsieckState();
var ecurr = currstate.en;
newstate.en = ecurr.Clone();
var xcurr = currstate.xn;
var x0 = currstate.xn;
var zcurr = (Matrix)currstate.zn.Clone();
var qcurr = currstate.qn;
var qmax = currstate.qmax;
var dt = currstate.dt;
var t = currstate.tn;
var z0 = (Matrix)currstate.zn.Clone();
//Tolerance computation factors
double Cq = Math.Pow(qcurr + 1, -1.0);
double tau = 1.0 / (Cq * Factorial(qcurr) * l[qcurr - 1][qcurr]);
int count = 0;
double Dq = 0.0, DqUp = 0.0, DqDown = 0.0;
double delta = 0.0;
//Scaling factors for the step size changing
//with new method order q' = q, q + 1, q - 1, respectively
double rSame, rUp, rDown;
var xprev = Vector.Zeros(n);
var gm = Vector.Zeros(n);
var deltaE = Vector.Zeros(n);
var M = Matrix.Identity(n, qmax - 1);
if (opts.SparseJacobian == null)
{
Matrix J = opts.Jacobian == null?NordsieckState.Jacobian(f, xcurr, t + dt) : opts.Jacobian;
Matrix P = Matrix.Identity(n, n) - J * dt * b[qcurr - 1];
do
{
xprev = xcurr.Clone();
gm = dt * f(t + dt, xcurr) - z0.CloneColumn(1) - ecurr;
ecurr = ecurr + P.SolveGE(gm);
xcurr = x0 + b[qcurr - 1] * ecurr;
//Row dimension is smaller than zcurr has
M = ecurr & l[qcurr - 1];
//So, "expand" the matrix
var MBig = Matrix.Identity(zcurr.RowDimension, zcurr.ColumnDimension);
for (int i = 0; i < zcurr.RowDimension; i++)
{
for (int j = 0; j < zcurr.ColumnDimension; j++)
{
MBig[i, j] = i < M.RowDimension && j < M.ColumnDimension ? M[i, j] : 0.0d;
}
}
zcurr = z0 + MBig;
Dq = ecurr.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xprev);
deltaE = ecurr - currstate.en;
deltaE *= (1.0 / (qcurr + 2) * l[qcurr - 1][qcurr - 1]);
DqUp = deltaE.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
DqDown = zcurr.CloneColumn(qcurr - 1).ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
delta = Dq / (tau / (2 * (qcurr + 2)));
count++;
} while (delta > 1.0d && count < opts.NumberOfIterations);
}
else
{
SparseMatrix J = opts.SparseJacobian;
SparseMatrix P = SparseMatrix.Identity(n, n) - J * dt * b[qcurr - 1];
do
{
xprev = xcurr.Clone();
gm = dt * f(t + dt, xcurr) - z0.CloneColumn(1) - ecurr;
ecurr = ecurr + P.SolveGE(gm);
xcurr = x0 + b[qcurr - 1] * ecurr;
//Row dimension is smaller than zcurr has
M = ecurr & l[qcurr - 1];
//So, "expand" the matrix
var MBig = Matrix.Identity(zcurr.RowDimension, zcurr.ColumnDimension);
for (int i = 0; i < zcurr.RowDimension; i++)
{
for (int j = 0; j < zcurr.ColumnDimension; j++)
{
MBig[i, j] = i < M.RowDimension && j < M.ColumnDimension ? M[i, j] : 0.0d;
}
}
zcurr = z0 + MBig;
Dq = ecurr.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xprev);
deltaE = ecurr - currstate.en;
deltaE *= (1.0 / (qcurr + 2) * l[qcurr - 1][qcurr - 1]);
DqUp = deltaE.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
DqDown = zcurr.CloneColumn(qcurr - 1).ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
delta = Dq / (tau / (2 * (qcurr + 2)));
count++;
} while (delta > 1.0d && count < opts.NumberOfIterations);
}
//======================================
var nsuccess = count < opts.NumberOfIterations ? currstate.nsuccess + 1 : 0;
if (count < opts.NumberOfIterations)
{
flag = false;
newstate.zn = (Matrix)zcurr.Clone();
newstate.xn = zcurr.CloneColumn(0);
newstate.en = ecurr.Clone();
}
else
{
flag = true;
newstate.zn = (Matrix)currstate.zn.Clone();
newstate.xn = currstate.zn.CloneColumn(0);
newstate.en = currstate.en.Clone();
}
//Compute step size scaling factors
rUp = 0.0;
if (currstate.qn < currstate.qmax)
{
rUp = rUp = 1.0 / 1.4 / (Math.Pow(DqUp, 1.0 / (qcurr + 2)) + 1e-6);
}
rSame = 1.0 / 1.2 / (Math.Pow(Dq, 1.0 / (qcurr + 1)) + 1e-6);
rDown = 0.0;
if (currstate.qn > 1)
{
rDown = 1.0 / 1.3 / (Math.Pow(DqDown, 1.0 / (qcurr)) + 1e-6);
}
//======================================
newstate.nsuccess = nsuccess >= currstate.qn ? 0 : nsuccess;
//Step size scale operations
if (rSame >= rUp)
{
if (rSame <= rDown && nsuccess >= currstate.qn && currstate.qn > 1)
{
newstate.qn = currstate.qn - 1;
newstate.Dq = DqDown;
for (int i = 0; i < n; i++)
{
for (int j = newstate.qn + 1; j < qmax + 1; j++)
{
newstate.zn[i, j] = 0.0;
}
}
nsuccess = 0;
newstate.rFactor = rDown;
}
else
{
newstate.qn = currstate.qn;
newstate.Dq = Dq;
newstate.rFactor = rSame;
}
}
else
{
if (rUp >= rDown)
{
if (rUp >= rSame && nsuccess >= currstate.qn && currstate.qn < currstate.qmax)
{
newstate.qn = currstate.qn + 1;
newstate.Dq = DqUp;
newstate.rFactor = rUp;
nsuccess = 0;
}
else
{
newstate.qn = currstate.qn;
newstate.Dq = Dq;
newstate.rFactor = rSame;
}
}
else
{
if (nsuccess >= currstate.qn && currstate.qn > 1)
{
newstate.qn = currstate.qn - 1;
newstate.Dq = DqDown;
for (int i = 0; i < n; i++)
{
for (int j = newstate.qn + 1; j < qmax + 1; j++)
{
newstate.zn[i, j] = 0.0;
}
}
nsuccess = 0;
newstate.rFactor = rDown;
}
else
{
newstate.qn = currstate.qn;
newstate.Dq = Dq;
newstate.rFactor = rSame;
}
}
}
newstate.qmax = qmax;
newstate.dt = dt;
newstate.tn = t;
return(newstate);
}
/// <summary>
/// Implementation of Gear's BDF method with dynamically changed step size and order. Order changes between 1 and 3.
/// </summary>
/// <param name="t0">Initial time point</param>
/// <param name="x0">Initial phase vector</param>
/// <param name="f">Right parts of the system</param>
/// <param name="opts">Options for accuracy control and initial step size</param>
/// <returns>Sequence of infinite number of solution points.</returns>
public static IEnumerable <SolPoint> GearBDF(double t0, Vector x0, Func <double, Vector, Vector> f, Options opts)
{
double t = t0;
Vector x = x0.Clone();
int n = x0.Length;
double tout = t0;
Vector xout = new Vector();
if (opts.OutputStep > 0) // Store previous solution point if OutputStep is specified (non-zero)
{
xout = x0.Clone();
tout += opts.OutputStep;
}
// Firstly, return initial point
yield return(new SolPoint(t0, x0.Clone()));
//Initial step size.
Vector dx = f(t0, x0).Clone();
double dt;
if (opts.InitialStep != 0)
{
dt = opts.InitialStep;
}
else
{
var tol = opts.RelativeTolerance;
var ewt = Vector.Zeros(n);
var ywt = Vector.Zeros(n);
var sum = 0.0;
for (int i = 0; i < n; i++)
{
ewt[i] = opts.RelativeTolerance * Math.Abs(x[i]) + opts.AbsoluteTolerance;
ywt[i] = ewt[i] / tol;
sum = sum + (double)dx[i] * dx[i] / (ywt[i] * ywt[i]);
}
dt = Math.Sqrt(tol / ((double)1.0d / (ywt[0] * ywt[0]) + sum / n));
}
dt = Math.Min(dt, opts.MaxStep);
var resdt = dt;
int qmax = 5;
int qcurr = 2;
//Compute Nordstieck's history matrix at t=t0;
Matrix zn = new Matrix(n, qmax + 1);
for (int i = 0; i < n; i++)
{
zn[i, 0] = x[i];
zn[i, 1] = dt * dx[i];
for (int j = qcurr; j < qmax + 1; j++)
{
zn[i, j] = 0.0d;
}
}
var eold = Vector.Zeros(n);
NordsieckState currstate = new NordsieckState();
currstate.delta = 0.0d;
currstate.Dq = 0.0d;
currstate.dt = dt;
currstate.en = eold;
currstate.tn = t;
currstate.xn = x0;
currstate.qn = qcurr;
currstate.qmax = qmax;
currstate.nsuccess = 0;
currstate.zn = zn;
currstate.rFactor = 1.0d;
bool isIterationFailed = false;
//Can produce any number of solution points
while (true)
{
// Reset fail flag
isIterationFailed = false;
// Predictor step
var z0 = currstate.zn.Clone();
currstate.zn = NordsieckState.ZNew(currstate.zn);
currstate.en = Vector.Zeros(n);
currstate.xn = currstate.zn.CloneColumn(0);
// Corrector step
currstate = PredictorCorrectorScheme(currstate, ref isIterationFailed, f, opts);
if (isIterationFailed) // If iterations are not finished - bad convergence
{
currstate.zn = z0;
currstate.nsuccess = 0;
currstate.ChangeStep();
}
else // Iterations finished
{
var r = Math.Min(1.1d, Math.Max(0.2d, currstate.rFactor));
if (currstate.delta >= 1.0d)
{
if (opts.MaxStep < Double.MaxValue)
{
r = Math.Min(r, opts.MaxStep / currstate.dt);
}
if (opts.MinStep > 0)
{
r = Math.Max(r, opts.MinStep / currstate.dt);
}
r = Math.Min(r, opts.MaxScale);
r = Math.Max(r, opts.MinScale);
currstate.dt = currstate.dt * r; // Decrease step
currstate.zn = NordsieckState.Rescale(currstate.zn, r);
}
else
{
// Output data
if (opts.OutputStep > 0) // Output points with specified step
{
while (currstate.tn <= tout && tout <= currstate.tn + currstate.dt)
{
yield return(new SolPoint(tout, Vector.Lerp(tout, currstate.tn,
xout, currstate.tn + currstate.dt, currstate.xn)));
tout += opts.OutputStep;
}
Vector.Copy(currstate.xn, xout);
}
else // Output each point
{
yield return(new SolPoint(currstate.tn + currstate.dt, currstate.xn));
}
currstate.tn = currstate.tn + currstate.dt;
if (opts.MaxStep < Double.MaxValue)
{
r = Math.Min(r, opts.MaxStep / currstate.dt);
}
if (opts.MinStep > 0)
{
r = Math.Max(r, opts.MinStep / currstate.dt);
}
r = Math.Min(r, opts.MaxScale);
r = Math.Max(r, opts.MinScale);
currstate.dt = currstate.dt * r;
currstate.zn = NordsieckState.Rescale(currstate.zn, r);
}
}
}
}
/// <summary>Solves system of linear equations Ax = b using Gaussian elimination with partial pivoting</summary>
/// <param name="a">Elements of matrix 'A'. This array is modified during solution!</param>
/// <param name="b">Right part 'b'. This array is also modified during solution!</param>
/// <returns>Solution of system 'x'</returns>
public static double[] SolveCore(double[][] a, double[] b)
{
if (a == null)
{
throw new ArgumentNullException("a");
}
if (b == null)
{
throw new ArgumentNullException("b");
}
int n = a.Length;
int[] map = Enumerable.Range(0, n).ToArray();
Vector x = Vector.Zeros(n);
for (int j = 0; j < n; j++)
{
// Find row with largest absolute value of j-st element
int maxIdx = 0;
for (int i = 0; i < n - j; i++)
{
if (Math.Abs(a[i][j]) > Math.Abs(a[maxIdx][j]))
{
maxIdx = i;
}
}
if (Math.Abs(a[maxIdx][j]) < 1e-12)
{
throw new InvalidOperationException("Cannot apply Gauss method");
}
// Divide this row by max value
for (int i = j + 1; i < n; i++)
{
a[maxIdx][i] /= a[maxIdx][j];
}
b[maxIdx] /= a[maxIdx][j];
a[maxIdx][j] = 1.0;
// Move this row to bottom
if (maxIdx != n - j - 1)
{
var temp = a[n - j - 1];
a[n - j - 1] = a[maxIdx];
a[maxIdx] = temp;
var temp3 = b[n - j - 1];
b[n - j - 1] = b[maxIdx];
b[maxIdx] = temp3;
var temp2 = map[n - j - 1];
map[n - j - 1] = map[maxIdx];
map[maxIdx] = temp2;
}
double[] an = a[n - j - 1];
// Process all other rows
for (int i = 0; i < n - j - 1; i++)
{
double[] aa = a[i];
if (aa[j] != 0)
{
for (int k = j + 1; k < n; k++)
{
aa[k] -= aa[j] * an[k];
}
b[i] -= aa[j] * b[n - j - 1];
aa[j] = 0;
}
}
}
// Build answer
for (int i = 0; i < n; i++)
{
double s = b[i];
for (int j = n - i; j < n; j++)
{
s -= x[j] * a[i][j];
}
x[n - i - 1] = s;
}
return(x);
}