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); }
private void SolveConstraints() { #region Contact and Joint elaboration //SaveShapePreviousProperties(collisionPoints); if (Partitions != null && Partitions.Any()) { var rangePartitioner = Partitioner.Create( 0, Partitions.Count, Convert.ToInt32(Partitions.Count / EngineParameters.MaxThreadNumber) + 1); Parallel.ForEach( rangePartitioner, new ParallelOptions { MaxDegreeOfParallelism = EngineParameters.MaxThreadNumber }, (range, loopState) => { for (int i = range.Item1; i < range.Item2; i++) { JacobianConstraint[] jacobianConstraints = GetJacobianConstraints( Partitions[i].PartitionedCollisionPoints.ToArray(), Partitions[i].PartitionedJoints, Shapes, EngineParameters); if (jacobianConstraints.Length > 0) { LinearProblemProperties LCP = GenerateLCP(jacobianConstraints); double[] overallSolution = Solver.Solve(LCP, jacobianConstraints, LCP.StartImpulse); //double[] overallSolution = Solve(jacobianConstraints); IntegrateVelocityEngine.UpdateVelocity(jacobianConstraints, overallSolution); } } }); } #endregion }