public Vector<double> Solve(DenseMatrix systemMatrix, Vector<double> freeTerms)
{
var determinant = systemMatrix.Determinant();
var determinants = new double[systemMatrix.ColumnCount];
var results = new double[systemMatrix.ColumnCount];
for (var i = 0; i < systemMatrix.ColumnCount; i++)
{
var matrix = systemMatrix.RemoveColumn(i).InsertColumn(i, freeTerms);
determinants[i] = matrix.Determinant();
}
for (var i = 0; i < determinants.Length; i++)
{
results[i] = determinants[i]/determinant;
}
return Vector.Build.Dense(results);
}
private DenseVector NewtonMethod(ref Parser[] parsers)
{
double eps = 10e-3;
//var helperMatrix = new DenseMatrix(equationsCount, equationsCount);
var helperMatrixR = new double[equationsCount, equationsCount];
for (int i = 0; i < equationsCount; i++)
{
for (int j = 0; j < equationsCount; j++)
{
double p;
double p1;
try
{
parsers[i].Variables[variables.ElementAt(j).Key] = variables.ElementAt(j).Value + eps;
p1 = parsers[i].Calculate();
parsers[i].Variables[variables.ElementAt(j).Key] = variables.ElementAt(j).Value;
p = parsers[i].Calculate();
}
catch(Exception exception)
{
ErrorAllert(exception.Message);
return null;
}
helperMatrixR[i, j] = (p1 - p)/eps;
}
}
var helperMatrix = new DenseMatrix(helperMatrixR);
if (Math.Abs(helperMatrix.Determinant()) < eps)
{
ErrorAllert("Determinant of Jacobi matrix is 0. Can't calculate result.");
return null;
}
var inverseMatrix = helperMatrix.Inverse();
var initVars = new DenseVector(equationsCount);
var modVars = new Dictionary<string, double>();
for (int i = 0; i < variables.Count; i++)
{
initVars[i] = variables.ElementAt(i).Value;
modVars[variables.ElementAt(i).Key] = variables.ElementAt(i).Value;
}
foreach (var parser in parsers)
{
parser.Variables = modVars;
}
bool diff = false;
int itCount = 0;
while (!diff && itCount < 10)
{
var f = new DenseVector(equationsCount);
for (int j = 0; j < equationsCount; j++)
{
f[j] = parsers[j].Calculate();
}
var result = new DenseVector((initVars - inverseMatrix*f).ToArray());
for (int i = 0; i < equationsCount; i++)
{
diff = diff && (Math.Abs(result[i] - initVars[i]) < eps);
}
initVars = result;
for (int j = 0; j < equationsCount; j++)
{
modVars[modVars.ElementAt(j).Key] = initVars[j];
}
++itCount;
}
return initVars;
}
/// <summary>
/// Run example
/// </summary>
/// <seealso cref="http://en.wikipedia.org/wiki/Determinant">Determinant</seealso>
/// <seealso cref="http://en.wikipedia.org/wiki/Rank_%28linear_algebra%29">Rank (linear algebra)</seealso>
/// <seealso cref="http://en.wikipedia.org/wiki/Trace_%28linear_algebra%29">Trace (linear algebra)</seealso>
/// <seealso cref="http://en.wikipedia.org/wiki/Condition_number">Condition number</seealso>
public void Run()
{
// Format matrix output to console
var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
formatProvider.TextInfo.ListSeparator = " ";
// Create random square matrix
var matrix = new DenseMatrix(5);
var rnd = new Random(1);
for (var i = 0; i < matrix.RowCount; i++)
{
for (var j = 0; j < matrix.ColumnCount; j++)
{
matrix[i, j] = rnd.NextDouble();
}
}
Console.WriteLine(@"Initial matrix");
Console.WriteLine(matrix.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 1. Determinant
Console.WriteLine(@"1. Determinant");
Console.WriteLine(matrix.Determinant());
Console.WriteLine();
// 2. Rank
Console.WriteLine(@"2. Rank");
Console.WriteLine(matrix.Rank());
Console.WriteLine();
// 3. Condition number
Console.WriteLine(@"2. Condition number");
Console.WriteLine(matrix.ConditionNumber());
Console.WriteLine();
// 4. Trace
Console.WriteLine(@"4. Trace");
Console.WriteLine(matrix.Trace());
Console.WriteLine();
}
//Given an observation, mu and sigma. What is N(O | mu, sigma)?
public double queryGuassian(DenseVector observation, DenseVector mean, DenseMatrix covar)
{
double scaletemp, scale, exponent, prob;
DenseMatrix v1, v2; //Temp matrices, for multiplying
scaletemp = (Math.Pow(2 * Math.PI, mean.Count / 2));
scale = (Math.Sqrt(covar.Determinant()));
scale *= scaletemp;
scale = 1 / scale;
v1 = (DenseMatrix)(observation - mean).ToRowMatrix();
v2 = (DenseMatrix)(observation - mean).ToColumnMatrix();
v2 = (DenseMatrix)(covar.Inverse()) * v2;
exponent = (-0.5) * ((v1 * v2).ToArray()[0,0]);
prob = scale * Math.Pow(Math.E, exponent);
return prob;
}
private static double LinePlaneIntersection(MathPoint origin, MathVector axis0, MathVector axis1, MathPoint lineOrigin, MathVector lineVector)
{
double x1, y1, z1;
double x2, y2, z2;
double x3, y3, z3;
double x4, y4, z4;
double x5, y5, z5;
double t;
var p0 = (MathPoint) origin.AddVector(axis0);
var p1 = (MathPoint) origin.AddVector(axis1);
Destructure(origin, out x1, out y1, out z1);
Destructure(p0, out x2, out y2, out z2);
Destructure(p1, out x3, out y3, out z3);
var p2 = (MathPoint) origin.AddVector(lineVector);
Destructure(lineOrigin, out x4, out y4, out z4);
Destructure(p2, out x5, out y5, out z5);
Matrix a = new DenseMatrix(4, 4, new double[]
{
1, x1, y1, z1, 1, x2, y2, z2, 1, x3, y3, z3, 1, x4, y4, z4
});
Matrix b = new DenseMatrix(4, 4, new double[]
{
1, x1, y1, z1, 1, x2, y2, z2, 1, x3, y3, z3, 0, x5 - x4, y5 - y4, z5 - z4
});
var da = a.Determinant();
var db = b.Determinant();
t = da/db;
return t;
}
