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); }