private double[] CalculateDirection(
LinearProblemProperties input,
double[] Xk1,
double[] deltaK,
ref double[] searchDirection,
double betak)
{
double[] result = new double[Xk1.Length];
for (int i = 0; i < Xk1.Length; i++)
{
double bDirection = betak * searchDirection[i];
result[i] = Xk1[i] + bDirection;
searchDirection[i] = bDirection - deltaK[i];
result[i] = ClampSolution.Clamp(input, result[i], ref result, i);
}
return(result);
}
private double ElaborateUpperTriangularMatrix(
LinearProblemProperties input,
double[] sum,
int[] solverOrder,
ref double[] x,
ref double[] oldX)
{
double error = 0.0;
for (int i = 0; i < input.Count; i++)
{
int index = solverOrder[i];
double sumBuffer = sum[index];
SparseVector m = input.M.Rows[index];
ShapeDefinition.ConstraintType cType = input.ConstraintType[index];
double xValue = x[index];
//Avoid first row elaboration
if (index != 0 && m.Index.Length > 0)
{
var bufValue = m.Value;
var bufIndex = m.Index;
for (int j = 0; j < bufIndex.Length; j++)
{
int idx = bufIndex[j];
if (idx < index)
{
sumBuffer += bufValue[j] * x[idx];
}
}
}
sumBuffer = (input.B[index] - sumBuffer) * input.InvD[index];
double sor = SolverParameters.SOR;
// Constraints with limit diverge with sor > 1.0
if (sor > 1.0 &&
(cType == ShapeDefinition.ConstraintType.Friction ||
cType == ShapeDefinition.ConstraintType.JointMotor ||
cType == ShapeDefinition.ConstraintType.JointLimit))
{
sor = 1.0;
}
xValue += (sumBuffer - xValue) * sor;
x[index] = ClampSolution.Clamp(input, xValue, ref x, index);
//Compute error
double diff = x[index] - oldX[index];
error += diff * diff;
oldX[index] = x[index];
}
return(error);
}