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 input,
JacobianConstraint[] constraints,
double[] x)
{
double[] Xk = gaussSeidelSolver.Solve(input, constraints, x);
double[] delta = CalculateDelta(Xk, x);
double[] searchDirection = NegateArray(delta);
double[] Xk1 = new double[input.Count];
double modDeltak = 0.0;
double oldModDeltak = 0.0;
for (int i = 0; i < solverParam.MaxIterations; i++)
{
Xk1 = gaussSeidelSolver.Solve(input, constraints, Xk);
double[] deltaK = CalculateDelta(Xk1, Xk);
modDeltak = ArraySquareModule(deltaK);
if (modDeltak < solverParam.ErrorTolerance)
{
break;
}
double betaK = (modDeltak * modDeltak) /
(oldModDeltak * oldModDeltak);
if (betaK > 1.0)
{
searchDirection = new double[searchDirection.Length];
}
else
{
Xk1 = CalculateDirection(
input,
Xk1,
deltaK,
ref searchDirection,
betaK);
}
Array.Copy(Xk1, Xk, Xk1.Length);
oldModDeltak = modDeltak;
}
deltaErrorCheck = modDeltak;
return(Xk1);
}
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);
}
