private double[] GetPhi(
LinearProblemProperties input,
SparseElement[] A,
double[] x,
double[] g)
{
double[] result = new double[input.Count];
for (int i = 0; i < input.Count; i++)
{
if (!ClampSolution.GetIfClamped(input, x, i))
result[i] = g[i];
else
{
double min = 0.0;
double max = 0.0;
ClampSolution.GetConstraintValues(input, x, i, ref min, ref max);
if (x[i] >= min && x[i] <= max)
{
result[i] = g[i];
}
else
{
result[i] = 0.0;
}
}
}
return result;
}
public static double ComputeSolverError(
LinearProblemProperties input,
SolutionValues[] X)
{
double error = 0.0;
for (int i = 0; i < input.Count; i++)
{
if (X[i].ConstraintStatus)
continue;
SparseElement m = input.M[i];
double[] bufValue = m.Value;
int[] bufIndex = m.Index;
double bValue = (1.0 / input.D[i]) * X[i].X;
for (int j = 0; j < m.Count; j++)
bValue += bufValue[j] * X[bufIndex[j]].X;
error += (bValue - input.B[i]) * (bValue - input.B[i]);
}
return error;
}
public SolutionValues[] Solve(
LinearProblemProperties input,
SolutionValues[] X = null)
{
if(X == null)
X = new SolutionValues[input.Count];
double internalSOR = SolverParameters.SOR;
double solverError = double.MaxValue;
for (int k = 0; k < SolverParameters.MaxIteration; k++)
{
double[] sum = ElaborateLowerTriangularMatrix(input, X);
for (int i = 0; i < input.Count; i++)
{
double sumBuffer = sum [i];
SparseElement m = input.M [i];
double[] bufValue = m.Value;
int[] bufIndex = m.Index;
//Avoid first row elaboration
if (i != 0)
{
for (int j = 0; j < m.Count; j++)
{
if (bufIndex[j] < i)
sumBuffer += bufValue [j] * X [bufIndex[j]].X;
}
}
sumBuffer = (input.B[i] - sumBuffer) * input.D[i];
X[i].X += (sumBuffer - X[i].X) * internalSOR;
X[i] = ClampSolution.Clamp (input, X, i);
sum[i] = sumBuffer;
}
if (SolverParameters.DynamicSORUpdate)
{
double actualSolverError = SolverHelper.ComputeSolverError(input, X);
if (actualSolverError > solverError)
internalSOR = Math.Max(internalSOR - SolverParameters.SORStep, SolverParameters.SORStep);
else
solverError = actualSolverError;
if (actualSolverError < SolverParameters.ErrorTolerance)
return X;
}
}
return X;
}
public SolutionValues[] Solve(
LinearProblemProperties linearProblemProperties,
SolutionValues[] X = null)
{
if (X == null)
X = new SolutionValues[linearProblemProperties.Count];
double[] x = new double[X.Length];
for (int i = 0; i < x.Length; i++)
x[i] = X[i].X;
SparseElement[] A = linearProblemProperties.GetOriginalMatrixSparse();
double[] g = Minus(Multiply(A, x), linearProblemProperties.B);
if (Dot(g, g) < 1E-50)
return X;
double[] pPhi = GetPhi(linearProblemProperties, A, x, g);
for (int i = 0; i < SolverParameters.MaxIteration; i++)
{
double alphaKDen = Dot(Multiply(A, pPhi), pPhi);
double alphaK = 0.0;
if (alphaKDen != 0)
alphaK = Dot(g, pPhi) / alphaKDen;
else
{
string ciao = "ciao";
}
double[] halfX = Minus(x, Multiply(alphaK, pPhi));
x = Project(linearProblemProperties, halfX);
g = Minus(Multiply(A, x), linearProblemProperties.B);
pPhi = GetPhi(linearProblemProperties, A, x, g);
}
for (int i = 0; i < x.Length; i++)
{
X[i].X = x[i];
X[i] = ClampSolution.Clamp(linearProblemProperties, X, i);
}
return X;
}
public SolutionValues[] Solve(
LinearProblemProperties input,
SolutionValues[] X = null)
{
if (X == null)
X = new SolutionValues[input.Count];
SolutionValues[] Xk = gaussSeidelSolver.Solve(input);
double[] delta = calculateDelta(Xk, X);
double[] searchDirection = negateArray(delta);
SolutionValues[] Xk1 = new SolutionValues[input.Count];
for (int i = 0; i < solverParam.MaxIteration; i++)
{
Xk1 = gaussSeidelSolver.Solve(input, Xk);
double[] deltaK = calculateDelta(Xk1, Xk);
deltaErrorCheck = arraySquareModule(delta);
double betaK = 1.1;
if (Math.Abs(deltaErrorCheck) > 1E-40)
betaK = arraySquareModule(deltaK) / deltaErrorCheck;
if (betaK > 1.0)
searchDirection = new double[searchDirection.Length];
else
{
Xk1 = calculateDirection (
input,
Xk1,
deltaK,
ref searchDirection,
betaK);
}
Array.Copy(Xk1, Xk, Xk1.Length);
Array.Copy(deltaK, delta, deltaK.Length);
}
return Xk1;
}
public static SolutionValues Clamp(
LinearProblemProperties input,
SolutionValues[] X,
int i)
{
switch (input.ConstraintType[i])
{
case ConstraintType.Collision:
case ConstraintType.JointLimit:
if (X[i].X < 0)
return new SolutionValues(0.0, true);
else
return new SolutionValues(X[i].X, false);
case ConstraintType.Friction:
double frictionLimit = X[input.Constraints[i][0].Value].X * input.ConstraintLimit[i];
if (X[i].X < -frictionLimit)
return new SolutionValues(-frictionLimit, true);
if (X[i].X > frictionLimit)
return new SolutionValues(frictionLimit, true);
return new SolutionValues(X[i].X, false);
case ConstraintType.JointMotor:
double limit = input.ConstraintLimit[i];
if (X[i].X < -limit)
return new SolutionValues(-limit, true);
if (X[i].X > limit)
return new SolutionValues(limit, true);
return new SolutionValues(X[i].X, false);
default:
return new SolutionValues(X[i].X, false);
}
}
public static double Clamp(
LinearProblemProperties input,
double[] X,
int i)
{
switch (input.ConstraintType[i])
{
case ConstraintType.Collision:
case ConstraintType.JointLimit:
if (X[i] < 0)
return 0.0;
else
return X[i];
case ConstraintType.Friction:
double frictionLimit = X[input.Constraints[i][0].Value] * input.ConstraintLimit[i];
if (X[i] < -frictionLimit)
return -frictionLimit;
if (X[i] > frictionLimit)
return frictionLimit;
return X[i];
case ConstraintType.JointMotor:
double limit = input.ConstraintLimit[i];
if (X[i] < -limit)
return -limit;
if (X[i] > limit)
return limit;
return X[i];
default:
return X[i];
}
}
public static bool GetIfClamped(
LinearProblemProperties input,
double[] X,
int i)
{
switch (input.ConstraintType[i])
{
case ConstraintType.Collision:
case ConstraintType.JointLimit:
if (Math.Abs(X[i]) < 1E-50)
return true;
else
return false;
case ConstraintType.Friction:
double frictionLimit = X[input.Constraints[i][0].Value] * input.ConstraintLimit[i];
if (Math.Abs(X[i] + frictionLimit) < 1E-50 ||
Math.Abs(X[i] - frictionLimit) < 1E-50)
return true;
else
return false;
case ConstraintType.JointMotor:
if (Math.Abs(X[i] - input.ConstraintLimit[i]) < 1E-50 ||
Math.Abs(X[i] + input.ConstraintLimit[i]) < 1E-50)
return true;
else
return false;
default:
return false;
}
}
public static void GetConstraintValues(
LinearProblemProperties input,
double[] x,
int i,
ref double Min,
ref double Max)
{
switch (input.ConstraintType[i])
{
case ConstraintType.Collision:
case ConstraintType.JointLimit:
Min = 0.0;
Max = double.MaxValue;
break;
case ConstraintType.Friction:
double frictionLimit = x[input.Constraints[i][0].Value] * input.ConstraintLimit[i];
Min = -frictionLimit;
Max = frictionLimit;
break;
case ConstraintType.JointMotor:
double limit = input.ConstraintLimit[i];
Min = -input.ConstraintLimit[i];
Max = input.ConstraintLimit[i];
break;
default:
Min = double.MinValue;
Max = double.MaxValue;
break;
}
}
public void Solve(LinearProblemProperties linearProblemProperties)
{
if (linearProblemProperties.Count > 0) {
double internalSOR = this.solverParameters.SOR;
double moduleOnlineGradient = 0.0;
double modOGa = 0.0;
double bk = 0.0;
double test = 0.0;
double[] oldX = new double[linearProblemProperties.Count];
double[] deltaf = new double[linearProblemProperties.Count];
double[] pk = new double[linearProblemProperties.Count];
double[] buffer = new double[linearProblemProperties.Count];
Array.Copy (linearProblemProperties.StartX, oldX, linearProblemProperties.Count);
this.gaussSeidel (
linearProblemProperties,
this.solverParameters.SOR,
ref buffer);
deltaf = this.calculateGradient (oldX, linearProblemProperties.StartX);
this.copyNegArray (ref pk, deltaf);
for (int i = 0; i < this.solverParameters.MaxIteration; i++) {
Array.Copy (linearProblemProperties.StartX, oldX, linearProblemProperties.Count);
moduleOnlineGradient = this.arrayModule (deltaf);
if (moduleOnlineGradient < this.solverParameters.ErrorTolerance)
break;
if (i == 0)
modOGa = moduleOnlineGradient;
else {
test = modOGa - moduleOnlineGradient;
if (test < 0.0 && internalSOR > 0.0)
internalSOR -= this.solverParameters.SORStep;
modOGa = moduleOnlineGradient;
}
this.gaussSeidel (
linearProblemProperties,
internalSOR,
ref buffer);
deltaf = this.calculateGradient (oldX, linearProblemProperties.StartX);
bk = 0.0;
if (moduleOnlineGradient != 0.0)
bk = this.arrayModule (deltaf) / moduleOnlineGradient;
else {
bk = 0.0;
continue;
}
if (bk <= 1.0) {
for (int j = 0; j < linearProblemProperties.Count; j++) {
linearProblemProperties.StartX [j] += bk * pk [j];
buffer [j] = oldX [j];
pk [j] = bk * pk [j] - deltaf [j];
this.clampX (linearProblemProperties, j);
}
}
}
}
}
private double kernel(
LinearProblemProperties input,
SolutionValues[] X,
int i)
{
double sumBuffer = 0.0;
//Avoid last row elaboration
if (i + 1 != input.Count)
{
double[] bufValue = input.M [i].Value;
int[] bufIndex = input.M [i].Index;
for (int j = 0; j < input.M [i].Count; j++) {
if(bufIndex [j] > i)
sumBuffer += bufValue [j] * X [bufIndex [j]].X;
}
}
return sumBuffer;
}
private SolutionValues[] calculateDirection(
LinearProblemProperties input,
SolutionValues[] Xk1,
double[] deltaK,
ref double[] searchDirection,
double betak)
{
SolutionValues[] result = new SolutionValues[Xk1.Length];
for (int i = 0; i < Xk1.Length; i++)
{
double bDirection = betak * searchDirection[i];
result[i].X = Xk1[i].X + bDirection;
searchDirection[i] = bDirection - deltaK[i];
result[i] = ClampSolution.Clamp(input, result, i);
}
return result;
}
private void internalIteration(
LinearProblemProperties input,
ref double[] sum,
ref double[] buffer,
double internalSOR,
int i)
{
int index = i * input.Count;
double sumBuffer = 0.0;
for (int j = 0; j < i; j++)
sumBuffer += input.M [index + j] * input.X [j];
sum [i] += sumBuffer;
sum [i] = (input.B [i] - sum [i]) * input.Diag [i];
input.X [i] = buffer [i] + (sum [i] - buffer [i]) * internalSOR;
}
private void clampX(
LinearProblemProperties input,
int i)
{
switch (input.ConstraintType[i])
{
case ConstraintType.Collision:
input.X [i] = Math.Max (0.0, input.X [i]);
break;
case ConstraintType.StaticFriction:
input.X [i] = GeometryUtilities.Clamp (
input.X [i],
input.X [i + input.Constraints[i]] * input.ConstraintLimit [i],
-input.X [i + input.Constraints[i]] * input.ConstraintLimit [i]);
break;
case ConstraintType.DynamicFriction:
input.X [i] = GeometryUtilities.Clamp (
input.X [i],
input.X [i + input.Constraints[i]] * input.ConstraintLimit [i],
-input.X [i + input.Constraints[i]] * input.ConstraintLimit [i]);
break;
case ConstraintType.Joint:
break;
default:
break;
}
}
private void gaussSeidel(
LinearProblemProperties input,
double internalSOR,
ref double[] buffer)
{
double[] sum = new double[input.Count];
this.lowerTriangularMatrix (input, ref sum);
for (int i = 0; i < input.Count; i++)
{
this.internalIteration (input, ref sum, ref buffer, internalSOR, i);
this.clampX (input,i);
buffer [i] = input.X [i];
}
}
private double[] Project(
LinearProblemProperties input,
double[] x)
{
double[] result = new double[input.Count];
for (int i = 0; i < input.Count; i++)
{
result[i] = ClampSolution.Clamp(input, x, i);
}
return result;
}
private SolutionValues[] BuildMatrixAndExecuteSolver(
JacobianContact[] contactConstraints,
LinearProblemProperties linearProblemProperties,
int nIterations)
{
if (linearProblemProperties != null)
{
solver.GetSolverParameters().SetSolverMaxIteration(nIterations);
SolutionValues[] solutionValues = solver.Solve(linearProblemProperties);
for (int j = 0; j < contactConstraints.Length; j++)
{
contactConstraints[j].StartImpulse.SetStartValue(solutionValues[j].X);
}
return solutionValues;
}
return null;
}
private void lowerTriangularMatrix(
LinearProblemProperties input,
ref double[] sum)
{
for (int i = 0; i < input.Count; i++)
sum[i] = this.kernel (input, i);
}
private double kernel(
LinearProblemProperties input,
int i)
{
double sumBuffer = 0.0;
int index = i * input.X.Length;
for (int j = 0; j < input.Count; j++)
sumBuffer += input.M [index + j] * input.X [j];
return sumBuffer;
}
private double[] ElaborateLowerTriangularMatrix(
LinearProblemProperties input,
SolutionValues[] X)
{
double[] sum = new double[input.Count];
Parallel.For (0,
input.Count,
new ParallelOptions { MaxDegreeOfParallelism = SolverParameters.MaxThreadNumber },
i => {
sum [i] = kernel (input, X, i);
});
return sum;
}
