public static MatrixN Transpose(MatrixN a)
{
MatrixN b = new MatrixN(a.Height, a.Width);
for (int row = 0; row < a.Height; row++)
{
for (int col = 0; col < a.Width; col++)
{
b.a[col, row] = a.a[row, col];
}
}
return(b);
}
public MatrixN Clone()
{
MatrixN r = new MatrixN(Width, Height);
for (int row = 0; row < Height; row++)
{
for (int col = 0; col < Width; col++)
{
r.a[row, col] = a[row, col];
}
}
return(r);
}
public static double[] Solve(FuncN[] funcs, double[] p)
{
for (int cnt = 0; cnt < 100; cnt++)
{
MatrixN y = new MatrixN(1, funcs.Length);
int zeros = 0;
for (int k = 0; k < funcs.Length; k++)
{
y.a[k, 0] = funcs[k].calc(p);
if (Math.Abs(y.a[k, 0]) < Epsilon)
{
zeros++;
}
}
if (zeros == funcs.Length)
{
break;
}
MatrixN j = new MatrixN(p.Length, funcs.Length);
for (int i = 0; i < p.Length; i++)
{
for (int k = 0; k < funcs.Length; k++)
{
j.a[k, i] = funcs[k].calcDer(i, p);
}
}
MatrixN jt = MatrixN.Transpose(j);
MatrixN gm = MatrixN.Multiply(jt, j);
MatrixN im = MatrixN.Invert(gm);
if (im == null)
{
for (int i = 0; i < p.Length; i++)
{
p[i] += 0.01;
}
continue;
}
MatrixN jp = MatrixN.Multiply(im, jt);
MatrixN dif = MatrixN.Multiply(jp, y);
for (int i = 0; i < p.Length; i++)
{
p[i] -= dif.a[i, 0];
}
}
return(p);
}
public static MatrixN Multiply(MatrixN a1, MatrixN b1)
{
if (a1.Width != b1.Height)
{
return(null);
}
MatrixN c = new MatrixN(b1.Width, a1.Height);
for (int i = 0; i < c.Height; i++)
{
for (int j = 0; j < c.Width; j++)
{
for (int k = 0; k < a1.Width; k++)
{
c.a[i, j] += a1.a[i, k] * b1.a[k, j];
}
}
}
return(c);
}
public static MatrixN Invert(MatrixN a)
{
a = a.Clone();
MatrixN b = new MatrixN(a.Width, a.Height);
for (int row = 0; row < a.Height; row++)
{
for (int col = 0; col < a.Width; col++)
{
if (row == col)
{
b.a[row, col] = 1;
}
}
}
for (int col2 = 0; col2 < a.Width; col2++)
{
if (Math.Abs(a.a[col2, col2]) < 0.000001)
{
bool found = false;
for (int row2 = col2 + 1; row2 < a.Height; row2++)
{
if (Math.Abs(a.a[row2, col2]) > 0.000001)
{
for (int col3 = 0; col3 < a.Width; col3++)
{
a.a[col2, col3] += a.a[row2, col3];
b.a[col2, col3] += b.a[row2, col3];
}
found = true;
break;
}
}
if (!found)
{
return(null);
}
}
for (int row2 = col2 + 1; row2 < a.Height; row2++)
{
double kofficient = -(a.a[row2, col2] / a.a[col2, col2]);
for (int col3 = 0; col3 < a.Width; col3++)
{
a.a[row2, col3] += a.a[col2, col3] * kofficient;
b.a[row2, col3] += b.a[col2, col3] * kofficient;
}
}
}
for (int col2 = a.Width - 1; col2 >= 0; col2--)
{
for (int row2 = col2 - 1; row2 >= 0; row2--)
{
double kofficient = -(a.a[row2, col2] / a.a[col2, col2]);
for (int col3 = 0; col3 < a.Width; col3++)
{
a.a[row2, col3] += a.a[col2, col3] * kofficient;
b.a[row2, col3] += b.a[col2, col3] * kofficient;
}
}
{
double kofficient = 1 / a.a[col2, col2];
for (int col3 = 0; col3 < a.Width; col3++)
{
a.a[col2, col3] *= kofficient;
b.a[col2, col3] *= kofficient;
}
}
}
return(b);
}
