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
}
