public static decimal[,] Cholesky(decimal[,] A)
{
int rows = A.GetLength(0);
int cols = A.GetLength(1);
decimal[,] L = new decimal[rows, cols];/*lower triangular*/
/*if (rows==cols & MathDecimal.Transpose(B)=B)*/
for (int k = 0; k < cols; k++)
{
decimal sum = 0;
for (int p = 0; p <= k - 2; p++)
{
sum += L[k, p] * L[k, p];
}
L[k, k] = MathDecimal.Sqrt(A[k, k] - sum);
for (int j = k; j < cols; j++)
{
sum = 0;
for (int p = 0; p <= k - 2; p++)
{
sum += L[k, p] * L[j, p];
}
A[j, k] = (A[k, j] - sum) / L[k, k];
}
}
return(L);
}
public override decimal[,] computeJ(decimal[] p)
{
//TODO: Rewrite computeJ!!!
decimal[,] M = { { p[0], p[1], p[2] }, { p[3], p[4], p[5] }, { p[6], p[7], p[8] } };
decimal[] b = { p[9], p[10], p[11] };
decimal[,] res = new decimal[data.Count * 2, 12];/*rewrite length ...*/
decimal B1, B2, B3, B, A;
for (int i = 0; i < res.GetLength(0) / 2; i++)
{
B1 = p[0] * data[i].data[0] + p[1] * data[i].data[1] + p[2] * data[i].data[2] + p[9];
B2 = p[3] * data[i].data[0] + p[4] * data[i].data[1] + p[5] * data[i].data[2] + p[10];
B3 = p[6] * data[i].data[0] + p[7] * data[i].data[1] + p[8] * data[i].data[2] + p[11];
B = B1 * B1 + B2 * B2 + B3 * B3;
A = B1 * B1 + B2 * B2;
res[2 * i, 0] = (180 * data[i].data[0] * B2) / (MathDecimal.PI * A);
res[2 * i, 1] = (180 * data[i].data[1] * B2) / (MathDecimal.PI * A);
res[2 * i, 2] = (180 * data[i].data[2] * B2) / (MathDecimal.PI * A);
res[2 * i, 3] = -(180 * data[i].data[0] * B1) / (MathDecimal.PI * A);
res[2 * i, 4] = -(180 * data[i].data[1] * B1) / (MathDecimal.PI * A);
res[2 * i, 5] = -(180 * data[i].data[2] * B1) / (MathDecimal.PI * A);
res[2 * i, 6] = 0;
res[2 * i, 7] = 0;
res[2 * i, 8] = 0;
res[2 * i, 9] = (180 * B2) / (MathDecimal.PI * A);
res[2 * i, 10] = -(180 * B1) / (MathDecimal.PI * A);
res[2 * i, 11] = 0;
res[2 * i + 1, 0] = -(180 * data[i].data[0] * B1 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 1] = -(180 * data[i].data[1] * B1 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 2] = -(180 * data[i].data[2] * B1 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 3] = -(180 * data[i].data[0] * B2 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 4] = -(180 * data[i].data[1] * B2 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 5] = -(180 * data[i].data[2] * B2 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 6] = (180 * data[i].data[0] * MathDecimal.Sqrt(A)) / (MathDecimal.PI * B);
res[2 * i + 1, 7] = (180 * data[i].data[1] * MathDecimal.Sqrt(A)) / (MathDecimal.PI * B);
res[2 * i + 1, 8] = (180 * data[i].data[2] * MathDecimal.Sqrt(A)) / (MathDecimal.PI * B);
res[2 * i + 1, 9] = -(180 * B1 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));
res[2 * i + 1, 10] = -(180 * B2 * B3) / (MathDecimal.PI * B * MathDecimal.Sqrt(A));;
res[2 * i + 1, 11] = (180 * MathDecimal.Sqrt(A)) / (MathDecimal.PI * B);
}
return(res);
}
public static decimal Norm2(decimal[] x)
{
return(MathDecimal.Sqrt(SquaredNorm2(x)));
}
