private LinearProblemProperties GenerateLCP(JacobianConstraint[] jacobianConstraints)
{
switch (solverType)
{
case SolverType.Lemke:
var baseLCP = LinearSystemBuilder.BuildLCP(jacobianConstraints, TimeStep);
return(LinearSystemBuilder.GetLCPFrictionMatrix(baseLCP));
case SolverType.ProjectedSymmetricGS:
case SolverType.ProjectedGaussSeidel:
case SolverType.NonLinearConjugateGradient:
case SolverType.RedBlackProjectedGaussSeidel:
default:
return(LinearSystemBuilder.BuildLCP(jacobianConstraints, TimeStep));
}
}
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);
}
