public double[] SolveExecute(
LinearProblemProperties input,
Dictionary <RedBlackEnum, List <int> > redBlackDictionary,
double[] x)
{
return(Execute(input, redBlackDictionary, x));
}
private double[] Execute(
LinearProblemProperties input,
double[] x,
int[] solverOrder)
{
double[] oldX = new double[x.Length];
double[] result = new double[x.Length];
Array.Copy(x, result, x.Length);
double actualSolverError = 0.0;
for (int k = 0; k < SolverParameters.MaxIterations; k++)
{
var sum = ElaborateLowerTriangularMatrix(input, result);
actualSolverError = ElaborateUpperTriangularMatrix(
input,
sum,
solverOrder,
ref result,
ref oldX);
if (actualSolverError < SolverParameters.ErrorTolerance)
{
break;
}
}
return(result);
}
public Dictionary <RedBlackEnum, List <int> > GetRedBlackDictionary(LinearProblemProperties input)
{
var graph = input.ConstrGraph;
var nodeDictionary = BreadthFirstSearch.GetBoolLevelBFS(graph, 0);
if (nodeDictionary.Count < input.Count)
{
for (int i = 0; i < input.Count; i++)
{
if (!nodeDictionary.ContainsKey(i))
{
var dict = BreadthFirstSearch.GetBoolLevelBFS(graph, i);
foreach (var element in dict)
{
nodeDictionary.Add(element.Key, element.Value);
}
if (nodeDictionary.Count == input.Count)
{
break;
}
}
}
}
var redBlackDictionary = GetRedBlackDictionary(nodeDictionary);
return(redBlackDictionary);
}
static double Kernel(
LinearProblemProperties input,
double[] x,
int i)
{
double sumBuffer = 0.0;
//Avoid last row elaboration
if (i + 1 != input.Count &&
input.M.Rows[i].Index.Length > 0)
{
var bufValue = input.M.Rows[i].Value;
var bufIndex = input.M.Rows[i].Index;
for (int j = 0; j < bufIndex.Length; j++)
{
int idx = bufIndex[j];
if (idx > i)
{
sumBuffer += bufValue[j] * x[idx];
}
}
}
return(sumBuffer);
}
public double[] Solve(
LinearProblemProperties input,
double[] x,
int[] solverOrder)
{
return(Execute(input, x, solverOrder));
}
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 static double ComputeSolverError(
LinearProblemProperties input,
double[] x)
{
double error = 0.0;
var counter = input.ConstraintType.Count(s => s != ShapeDefinition.ConstraintType.Friction);
if (counter > 0)
{
for (int i = 0; i < input.Count; i++)
{
SparseVector m = input.M.Rows[i];
if (input.ConstraintType[i] != ShapeDefinition.ConstraintType.Friction)
{
var bufValue = m.Value;
var bufIndex = m.Index;
double bValue = (1.0 / input.InvD[i]) * x[i];
for (int j = 0; j < bufIndex.Length; j++)
{
bValue += bufValue[j] * x[bufIndex[j]];
}
error += (bValue - input.B[i]) * (bValue - input.B[i]);
}
}
return(error / counter);
}
return(-1.0);
}
public double[] Solve(
LinearProblemProperties input,
JacobianConstraint[] constraints,
double[] x)
{
var solverOrder = Enumerable.Range(0, x.Length).ToArray();
return(Execute(input, x, solverOrder));
}
public double[] Solve(
LinearProblemProperties input,
JacobianConstraint[] constraints,
double[] x)
{
var redBlackDictionary = GetRedBlackDictionary(input);
return(Execute(input, redBlackDictionary, x));
}
private double[] ElaborateLowerTriangularMatrix(
LinearProblemProperties input,
double[] x)
{
double[] sum = new double[input.Count];
for (int i = 0; i < input.Count; i++)
{
sum[i] = Kernel(input, x, i);
}
return(sum);
}
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);
}
private void UpdateFrictionConstraints(LinearProblemProperties lcp)
{
var frictionRes = new int?[lcp.Count];
for (int i = 0; i < lcp.Constraints.Length; i++)
{
if (lcp.ConstraintType[i] == ConstraintType.Collision)
{
for (int j = 0; j < lcp.FrictionDirections; j++)
{
frictionRes[i - j - 1] = -i;
}
}
}
lcp.SetConstraints(frictionRes);
}
public static bool GetIfClamped(
LinearProblemProperties input,
double[] X,
int i)
{
switch (input.ConstraintType[i])
{
case ConstraintType.Collision:
case ConstraintType.JointLimit:
return(true);
case ConstraintType.Friction:
return(true);
case ConstraintType.JointMotor:
return(true);
default:
return(false);
}
}
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);
}
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);
}
public double[] Solve(
LinearProblemProperties linearProblemProperties,
JacobianConstraint[] constraints,
double[] x)
{
double lambda = 1.0;
var A = linearProblemProperties.GetOriginalSparseMatrix();
var b = linearProblemProperties.B;
int N = b.Length;
double gamma = 1E-28;
double error = double.PositiveInfinity;
double oldError = 0.0;
for (int iter = 0; iter < SolverParameters.MaxIterations; iter++)
{
double[] y = Add(SparseMatrix.Multiply(A, x, SolverParameters.MaxThreadNumber), b);
double[] phi = PhiLambda(y, x, lambda);
oldError = error;
error = 0.5 * Dot(phi, phi);
var fnd = Find(phi, x, gamma);
List <int> S = fnd.Item1;
List <int> I = fnd.Item2;
int restart = Math.Min(A.n, 10);
double[] nablaPhi = null;
//TODO test other function
var dx = RandomFunc(A, x, y, phi, I, N, restart, ref nablaPhi);
x = ArmijoBacktracking(A, b, x, dx, nablaPhi, lambda, gamma, error);
}
return(x);
}
private void ExecuteKernel(
LinearProblemProperties input,
int index,
ref double[] x,
object sync)
{
SparseVector m = input.M.Rows[index];
double xValue = x[index];
double sumBuffer = 0.0;
var bufValue = m.Value;
var bufIndex = m.Index;
for (int j = 0; j < bufIndex.Length; j++)
{
int idx = bufIndex[j];
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 &&
(input.ConstraintType[index] == ConstraintType.Friction ||
input.ConstraintType[index] == ConstraintType.JointMotor ||
input.ConstraintType[index] == ConstraintType.JointLimit))
{
sor = 1.0;
}
xValue += (sumBuffer - xValue) * sor;
x[index] = SetValue(input, xValue, ref x, index, sync);
}
private double SetValue(
LinearProblemProperties input,
double value,
ref double[] x,
int i,
object sync)
{
lock (sync)
{
switch (input.ConstraintType[i])
{
case ConstraintType.Collision:
case ConstraintType.JointLimit:
return((value < 0.0) ?
0.0 :
value);
case ConstraintType.Friction:
//Isotropic friction -> sqrt(c1^2+c2^2) <= fn*U
int normalIndex = input.Constraints[i].Value;
double frictionLimit = x[normalIndex] * input.ConstraintLimit[i];
if (frictionLimit == 0.0)
{
x[normalIndex + 1] = 0.0;
x[normalIndex + 2] = 0.0;
return(0.0);
}
double directionA = x[normalIndex + 1];
double directionB = x[normalIndex + 2];
double frictionValue = Math.Sqrt(directionA * directionA + directionB * directionB);
if (frictionValue > frictionLimit)
{
Vector2d frictionNormal = new Vector2d(directionA / frictionValue, directionB / frictionValue);
x[normalIndex + 1] = frictionNormal.x * frictionLimit;
x[normalIndex + 2] = frictionNormal.y * frictionLimit;
return((i - normalIndex == 1) ?
x[normalIndex + 1] :
x[normalIndex + 2]);
}
return(value);
case ConstraintType.JointMotor:
double limit = input.ConstraintLimit[i];
if (value < -limit)
{
return(-limit);
}
if (value > limit)
{
return(limit);
}
return(value);
default:
return(value);
}
}
}
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);
}
public static double Clamp(
LinearProblemProperties input,
double xValue,
ref double[] X,
int i)
{
switch (input.ConstraintType[i])
{
case ConstraintType.Collision:
case ConstraintType.JointLimit:
return((xValue < 0.0) ?
0.0:
xValue);
case ConstraintType.Friction:
//Isotropic friction -> sqrt(c1^2+c2^2) <= fn*U
int sign = (input.Constraints[i].Value < 0) ? -1 : 1;
int normalIndex = sign * input.Constraints[i].Value;
double frictionLimit = X[normalIndex] * input.ConstraintLimit[i];
int idx1 = normalIndex + sign;
int idx2 = normalIndex + sign * 2;
if (frictionLimit == 0.0)
{
X[idx1] = 0.0;
X[idx2] = 0.0;
return(0.0);
}
double directionA = X[idx1];
double directionB = X[idx2];
double frictionValue = Math.Sqrt(directionA * directionA + directionB * directionB);
if (frictionValue > frictionLimit)
{
Vector2d frictionNormal = new Vector2d(directionA / frictionValue, directionB / frictionValue);
X[idx1] = frictionNormal.x * frictionLimit;
X[idx2] = frictionNormal.y * frictionLimit;
return((Math.Abs(i - normalIndex) == 1) ?
X[idx1] :
X[idx2]);
}
return(xValue);
case ConstraintType.JointMotor:
double limit = input.ConstraintLimit[i];
if (xValue < -limit)
{
return(-limit);
}
if (xValue > limit)
{
return(limit);
}
return(xValue);
default:
return(xValue);
}
}
private double[] Execute(
LinearProblemProperties input,
Dictionary <RedBlackEnum, List <int> > redBlackDictionary,
double[] x)
{
double[] oldX = new double[x.Length];
double[] result = new double[x.Length];
Array.Copy(x, result, x.Length);
double actualSolverError = 0.0;
redBlackDictionary.TryGetValue(RedBlackEnum.Red, out List <int> red);
redBlackDictionary.TryGetValue(RedBlackEnum.Black, out List <int> black);
OrderablePartitioner <Tuple <int, int> > rangePartitionerBlack = null;
OrderablePartitioner <Tuple <int, int> > rangePartitionerRed = null;
try
{
if (black.Any())
{
rangePartitionerBlack = Partitioner.Create(0, black.Count, Convert.ToInt32(black.Count / SolverParameters.MaxThreadNumber) + 1);
}
if (red.Any())
{
rangePartitionerRed = Partitioner.Create(0, red.Count, Convert.ToInt32(red.Count / SolverParameters.MaxThreadNumber) + 1);
}
for (int k = 0; k < SolverParameters.MaxIterations; k++)
{
var sync = new object();
if (red.Any())
{
//Execute Red
Parallel.ForEach(
rangePartitionerRed,
new ParallelOptions {
MaxDegreeOfParallelism = SolverParameters.MaxThreadNumber
},
(range, loopState) =>
{
for (int i = range.Item1; i < range.Item2; i++)
{
ExecuteKernel(input, red[i], ref result, sync);
}
});
}
if (black.Any())
{
//Execute Black
Parallel.ForEach(
rangePartitionerBlack,
new ParallelOptions {
MaxDegreeOfParallelism = SolverParameters.MaxThreadNumber
},
(range, loopState) =>
{
for (int i = range.Item1; i < range.Item2; i++)
{
ExecuteKernel(input, black[i], ref result, sync);
}
});
}
actualSolverError = SolverHelper.ComputeSolverError(result, oldX);
if (actualSolverError < SolverParameters.ErrorTolerance)
{
break;
}
Array.Copy(result, oldX, x.Length);
}
}
catch (Exception ex)
{
throw new Exception(ex.Message);
}
return(result);
}
