private static void FormOutput(MathNet.Numerics.LinearAlgebra.Vector <double> solution, Equations.Nonlinear.NonlinearEquationDescription system, ref List <string> outputList)
{
for (int i = 0; i < solution.Count; i++)
{
outputList.Add(system.VariableNames[i] + " = " + solution[i].ToString());
}
double[] x = solution.ToArray();
for (int i = 0; i < system.Equations.Count; i++)
{
outputList.Add($"F{i}(X) = {system.Equations[i].Execute(x)}");
}
}
public static Vector2 trilaterate2DLinear(List <BleNode> nodes, List <double> distances)
{
if (nodes.Count == 3 && distances.Count == 3)
{
MathNet.Numerics.LinearAlgebra.Vector <double> vA = MathNet.Numerics.LinearAlgebra.Vector <double> .Build.Dense(new double[] { nodes[0].X, nodes[0].Y });
double[] b = { 0, 0 };
b[0] = 0.5 * (Math.Pow(distances[0], 2) - Math.Pow(distances[1], 2) + Math.Pow(getDistance(nodes[1], nodes[0]), 2));
b[1] = 0.5 * (Math.Pow(distances[0], 2) - Math.Pow(distances[2], 2) + Math.Pow(getDistance(nodes[2], nodes[0]), 2));
MathNet.Numerics.LinearAlgebra.Vector <double> vb = MathNet.Numerics.LinearAlgebra.Vector <double> .Build.Dense(b);
double[,] A = { { nodes[1].X - nodes[0].X, nodes[1].Y - nodes[0].Y }, { nodes[2].X - nodes[0].X, nodes[2].Y - nodes[0].Y } };
MathNet.Numerics.LinearAlgebra.Matrix <double> mA = MathNet.Numerics.LinearAlgebra.Matrix <double> .Build.DenseOfArray(A);
MathNet.Numerics.LinearAlgebra.Matrix <double> mAT = mA.Transpose();
MathNet.Numerics.LinearAlgebra.Vector <double> x = MathNet.Numerics.LinearAlgebra.Vector <double> .Build.Dense(2);
double det = mA.Multiply(mAT).Determinant();
if (det > 0.1)
{
x = (mA.Transpose() * mA).Inverse() * (mA.Transpose() * vb);
}
else
{
x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
}
x.Add(vA);
double[] coordinates = x.ToArray();
Vector2 vector = new Vector2((float)coordinates[0], (float)coordinates[1]);
return(vector);
}
return(new Vector2());
}