public double[] Solve(
LinearProblemProperties input,
JacobianConstraint[] constraints,
double[] x)
{
double[] oldX = new double[x.Length];
double[] result = new double[x.Length];
Array.Copy(x, result, x.Length);
double actualSolverError = 0.0;
result = gaussSeidelSolver.Solve(input, constraints, oldX);
for (int i = 0; i < solverParam.MaxIterations; i++)
{
velocityIntegration.UpdateVelocity(constraints, result);
input = lcpEngine.UpdateConstantsTerm(constraints, input);
result = gaussSeidelSolver.Solve(input, constraints, new double[x.Length]);
actualSolverError = SolverHelper.ComputeSolverError(result, oldX);
if (actualSolverError < solverParam.ErrorTolerance)
{
break;
}
Array.Copy(result, oldX, x.Length);
}
return(result);
}
public double[] Solve(
LinearProblemProperties linearProblemProperties,
JacobianConstraint[] constraints,
double[] x)
{
JacobianConstraint[] constraintsRev = new JacobianConstraint[constraints.Length];
Array.Copy(constraints, constraintsRev, constraints.Length);
Array.Reverse(constraintsRev);
var symLCP = lcpEngine.BuildLCP(constraintsRev, 0.015);
UpdateFrictionConstraints(symLCP);
double[] oldX = new double[x.Length];
double[] result = new double[x.Length];
double[] resultRev = new double[x.Length];
Array.Copy(x, result, x.Length);
double actualSolverError = 0.0;
for (int i = 0; i < SolverParameters.MaxIterations; i++)
{
result = gsSolver.Solve(linearProblemProperties, constraints, result);
Array.Reverse(result);
result = gsSolver.Solve(symLCP, constraints, result);
Array.Reverse(result);
actualSolverError = SolverHelper.ComputeSolverError(result, oldX);
if (actualSolverError < SolverParameters.ErrorTolerance)
{
break;
}
Array.Copy(result, oldX, x.Length);
}
return(result);
}
private double[] Execute(
LinearProblemProperties input,
Dictionary <RedBlackEnum, List <int> > redBlackDictionary,
double[] x)
{
double[] oldX = new double[x.Length];
double[] result = new double[x.Length];
Array.Copy(x, result, x.Length);
double actualSolverError = 0.0;
redBlackDictionary.TryGetValue(RedBlackEnum.Red, out List <int> red);
redBlackDictionary.TryGetValue(RedBlackEnum.Black, out List <int> black);
OrderablePartitioner <Tuple <int, int> > rangePartitionerBlack = null;
OrderablePartitioner <Tuple <int, int> > rangePartitionerRed = null;
try
{
if (black.Any())
{
rangePartitionerBlack = Partitioner.Create(0, black.Count, Convert.ToInt32(black.Count / SolverParameters.MaxThreadNumber) + 1);
}
if (red.Any())
{
rangePartitionerRed = Partitioner.Create(0, red.Count, Convert.ToInt32(red.Count / SolverParameters.MaxThreadNumber) + 1);
}
for (int k = 0; k < SolverParameters.MaxIterations; k++)
{
var sync = new object();
if (red.Any())
{
//Execute Red
Parallel.ForEach(
rangePartitionerRed,
new ParallelOptions {
MaxDegreeOfParallelism = SolverParameters.MaxThreadNumber
},
(range, loopState) =>
{
for (int i = range.Item1; i < range.Item2; i++)
{
ExecuteKernel(input, red[i], ref result, sync);
}
});
}
if (black.Any())
{
//Execute Black
Parallel.ForEach(
rangePartitionerBlack,
new ParallelOptions {
MaxDegreeOfParallelism = SolverParameters.MaxThreadNumber
},
(range, loopState) =>
{
for (int i = range.Item1; i < range.Item2; i++)
{
ExecuteKernel(input, black[i], ref result, sync);
}
});
}
actualSolverError = SolverHelper.ComputeSolverError(result, oldX);
if (actualSolverError < SolverParameters.ErrorTolerance)
{
break;
}
Array.Copy(result, oldX, x.Length);
}
}
catch (Exception ex)
{
throw new Exception(ex.Message);
}
return(result);
}
