public double[] GetOtherRHSComponents(ISolverSubdomain subdomain, IVector <double> currentSolution)
{
//// u[id]: old solution
//// v[id]: current solution
//// vv: old acceleration
//// v2: current acceleration
//// v1: current velocity
////double vv = v2[id].Data[j];
////v2[id].Data[j] = a0 * (v[id].Data[j] - u[id].Data[j]) - a2 * v1[id].Data[j] - a3 * vv;
////v1[id].Data[j] += a6 * vv + a7 * v2[id].Data[j];
//int id = subdomain.ID;
//Vector<double> currentAcceleration = new Vector<double>(subdomain.Solution.Length);
//Vector<double> currentVelocity = new Vector<double>(subdomain.Solution.Length);
//Vector<double> uu = new Vector<double>(subdomain.Solution.Length);
//Vector<double> uc = new Vector<double>(subdomain.Solution.Length);
//for (int j = 0; j < subdomain.RHS.Length; j++)
//{
// currentAcceleration.Data[j] = a0 * (currentSolution[j] - v[id].Data[j]) - a2 * v1[id].Data[j] - a3 * v2[id].Data[j];
// currentVelocity.Data[j] = v1[id].Data[j] + a6 * v2[id].Data[j] + a7 * currentAcceleration.Data[j];
// uu.Data[j] = a0 * currentSolution[j] + a2 * currentVelocity.Data[j] + a3 * currentAcceleration.Data[j];
// uc.Data[j] = a1 * currentSolution[j] + a4 * currentVelocity.Data[j] + a5 * currentAcceleration.Data[j];
//}
//Vector<double> tempResult = new Vector<double>(subdomain.Solution.Length);
//Vector<double> result = new Vector<double>(subdomain.Solution.Length);
//provider.MassMatrixVectorProduct(subdomain, uu, tempResult.Data);
//result.Add(tempResult);
//provider.DampingMatrixVectorProduct(subdomain, uc, tempResult.Data);
//result.Add(tempResult);
//return result.Data;
Vector <double> result = new Vector <double>(subdomain.Solution.Length);
Vector <double> temp = new Vector <double>(subdomain.Solution.Length);
Vector <double> tempResult = new Vector <double>(subdomain.Solution.Length);
//Vector<double> uu = new Vector<double>(subdomain.Solution.Length);
//Vector<double> uc = new Vector<double>(subdomain.Solution.Length);
//int id = subdomain.ID;
//for (int j = 0; j < subdomain.RHS.Length; j++)
//{
// uu.Data[j] = -a0 * (v[id].Data[j] - currentSolution[j]) - a2 * v1[id].Data[j] - a3 * v2[id].Data[j];
// uc.Data[j] = -a1 * (v[id].Data[j] - currentSolution[j]) - a4 * v1[id].Data[j] - a5 * v2[id].Data[j];
//}
//provider.MassMatrixVectorProduct(subdomain, uu, tempResult.Data);
//result.Add(tempResult);
//provider.DampingMatrixVectorProduct(subdomain, uc, tempResult.Data);
//result.Add(tempResult);
////CalculateRHSImplicit(subdomain, result.Data, false);
////result.Scale(-1d);
currentSolution.CopyTo(temp.Data, 0);
temp.Scale(a0);
provider.MassMatrixVectorProduct(subdomain, temp, tempResult.Data);
result.Add(tempResult);
currentSolution.CopyTo(temp.Data, 0);
temp.Scale(a1);
provider.DampingMatrixVectorProduct(subdomain, temp, tempResult.Data);
result.Add(tempResult);
return(result.Data);
}
/// <summary>
/// Resolves the variables.
/// </summary>
public void Solve()
{
BuildSolver();
// Solve for real this time
var iterations = 0;
double error = 1.0;
bool needsReordering = true;
while (error > 1e-9)
{
// Build the matrix
_solver.Reset();
foreach (var eq in _map)
{
eq.Key.Value = _solution[eq.Value];
}
foreach (var eq in _equations)
{
eq.Value.Update();
eq.Value.Apply(1.0, null);
}
iterations++;
// Order and factor
int steps;
_solver.Degeneracy = 0;
if (needsReordering)
{
steps = _solver.OrderAndFactor();
needsReordering = false;
}
else
{
if (!_solver.Factor())
{
needsReordering = true;
continue;
}
else
{
steps = _solution.Length;
}
}
// Update the solution
if (LogInfo)
{
Console.WriteLine($"- Iteration {iterations} ({steps}/{_solver.Size} rows factored)");
if (steps < _solution.Length)
{
for (var i = steps + 1; i <= _solution.Length; i++)
{
var ext = _solver.InternalToExternal(new MatrixLocation(i, i));
Console.WriteLine($" (Could not solve equation for {_map.Reverse(ext.Row)} or unknown {_map.Reverse(ext.Column)})");
}
}
}
_solver.Degeneracy = _solver.Size - steps;
_solution.CopyTo(_oldSolution);
_solver.Solve(_solution);
if (LogInfo)
{
foreach (var eq in _map)
{
var msg = $" {eq.Key} = {_solution[eq.Value]} (last was {_oldSolution[eq.Value]})";
if (Math.Abs(_solution[eq.Value] - _oldSolution[eq.Value]) > 1e-6)
{
Utility.Error(msg);
}
else
{
Console.WriteLine(msg);
}
}
}
// find the maximum error
error = 0.0;
for (var i = 1; i <= _map.Count; i++)
{
// Detect infinity and NaN
var uk = _map.Reverse(i);
if (_lambdas.Contains(uk))
{
// We don't particularly care of our Lagrange multipliers, we will just make sure that they don't cause problems later on
// The thing is that lagrange multipliers can become undefined if a minimization and a constraint
// are leading to the same solution. In that case we get lamba*0=0 and the lagrange multiplier becomes undefined.
if (double.IsNaN(_solution[i]) || double.IsPositiveInfinity(_solution[i]) || _solution[i] > 1e6)
{
_solution[i] = 1e6; // Just a value to get through the next iteration
}
else if (double.IsNegativeInfinity(_solution[i]) || _solution[i] < -1e6)
{
_solution[i] = -1e6;
}
}
else
{
// For any other unknown, we do want to know how far we're off
double e;
switch (uk.Type)
{
case UnknownTypes.Length:
e = Math.Abs(_solution[i] - _oldSolution[i]);
// Add some randomization to avoid invalid "local minimum" solutions
if (_solution[i] < 0)
{
_solution[i] = 1e-6 / (_rnd.NextDouble() + 1);
}
error = Math.Max(error, e);
break;
case UnknownTypes.Scale:
e = Math.Abs(_solution[i] - _oldSolution[i]);
// Add some random variation to stay away from zero to avoid invalid "local minimum" solutions
if (_solution[i].IsZero())
{
var r = _rnd.NextDouble();
if (r > 0.5)
{
_solution[i] = 1e-3 / (r + 1.0);
}
else
{
_solution[i] = -1e-3 / (r + 1.0);
}
}
error = Math.Max(error, e);
break;
default:
e = Math.Abs(_solution[i] - _oldSolution[i]);
error = Math.Max(error, e);
break;
}
}
}
// Take a partial step if necessary to keep our inequalities greater than their minimum value
double alpha = 1;
foreach (var m in _minimum)
{
if (_map.TryGet(m.Key, out var index))
{
if (_solution[index] < m.Value)
{
alpha = Math.Min(alpha, (m.Value - _oldSolution[index]) / (_solution[index] - _oldSolution[index]));
error = 1;
}
if (_solution[index] <= m.Value)
{
_solution[index] = m.Value + 1e-9;
}
}
}
if (LogInfo)
{
Console.WriteLine($"Alpha = {alpha}");
}
for (var i = 1; i <= _map.Count; i++)
{
_solution[i] = _solution[i] * alpha + _oldSolution[i] * (1 - alpha);
}
if (error < 1e-6)
{
break;
}
if (iterations > 100)
{
if (LogInfo)
{
Console.WriteLine("Could not converge...");
}
Warn(this, new WarningEventArgs("Could not converge to a solution."));
return;
}
}
}
public void Solve(IVector <double> f, double[] result)
{
//var e = DateTime.Now;
SkylineMatrix2D <T> K = this;
if (!K.isFactorized)
{
throw new InvalidOperationException("Cannot solve if matrix is not factorized.");
}
if (!(typeof(T) == typeof(double)))
{
throw new InvalidOperationException("Cannot solve for types other than double");
}
if (K.Rows != f.Length)
{
throw new InvalidOperationException("Matrix and vector size mismatch.");
}
double[] d = K.Data as double[];
//double[] result = new double[K.Rows];
f.CopyTo(result, 0);
// RHS vector reduction
int n;
for (n = 0; n < K.Rows; n++)
{
int KL = K.RowIndex[n] + 1;
int KU = K.RowIndex[n + 1] - 1;
if (KU >= KL)
{
int k = n;
double C = 0;
for (int KK = KL; KK <= KU; KK++)
{
k--;
C += d[KK] * result[k];
}
result[n] -= C;
}
}
// Back substitution
for (n = 0; n < K.Rows; n++)
{
result[n] /= d[K.RowIndex[n]];
}
n = K.Rows - 1;
for (int l = 1; l < K.Rows; l++)
{
int KL = K.RowIndex[n] + 1;
int KU = K.RowIndex[n + 1] - 1;
if (KU >= KL)
{
int k = n;
for (int KK = KL; KK <= KU; KK++)
{
k--;
result[k] -= d[KK] * result[n];
}
}
n--;
}
//var x = new List<TimeSpan>();
//x.Add(DateTime.Now - e);
}
