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