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 LinearProblemProperties (
SparseElement[] M,
double[] B,
SolutionValues[] startX,
double[] d,
double[] constraintLimit,
ConstraintType[] constraintType,
int?[][] constraints,
int count)
{
this.M = M;
this.B = B;
D = d;
ConstraintLimit = constraintLimit;
ConstraintType = constraintType;
Constraints = constraints;
Count = count;
}
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);
}
}
private double[] calculateDelta(
SolutionValues[] a,
SolutionValues[] b)
{
if (a.Length < 0 ||
b.Length < 0 ||
b.Length != a.Length)
{
throw new Exception("Different array size.");
}
double[] result = new double[a.Length];
for (int i = 0; i < a.Length; i++)
result[i] = -(a[i].X - b[i].X);
return result;
}
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;
}
/// <summary>
/// Builds the LCP matrix for solver.
/// </summary>
private LinearProblemProperties BuildLCPMatrix(
JacobianContact[] contact,
bool positionStabilization = false)
{
if (contact.Length > 0)
{
SparseElement[] M = new SparseElement[contact.Length];
double[] B = new double[contact.Length];
SolutionValues[] X = new SolutionValues[contact.Length];
double[] D = new double[contact.Length];
ConstraintType[] constraintsType = new ConstraintType[contact.Length];
double[] constraintsLimit = new double[contact.Length];
List<int?>[] constraints = new List<int?>[contact.Length];
List<int>[] index = new List<int>[contact.Length];
List<double>[] value = new List<double>[contact.Length];
for (int i = 0; i < contact.Length; i++)
{
index [i] = new List<int> ();
value [i] = new List<double> ();
constraints[i] = new List<int?>();
}
//Critical section variable
var sync = new object ();
Parallel.For (0,
contact.Length,
new ParallelOptions { MaxDegreeOfParallelism = SimulationEngineParameters.MaxThreadNumber },
i => {
JacobianContact contactA = contact [i];
if (positionStabilization)
B[i] = contactA.CorrectionValue;
else
B[i] = -(contactA.B - ((contactA.CorrectionValue) < 0 ? Math.Max(contactA.CorrectionValue, -SimulationEngineParameters.MaxCorrectionValue):
Math.Min(contactA.CorrectionValue, SimulationEngineParameters.MaxCorrectionValue)));
X[i].X = contactA.StartImpulse.StartImpulseValue;
if (contactA.ContactReference.HasValue)
constraints[i].Add(contactA.ContactReference);
constraintsLimit [i] = contactA.ConstraintLimit;
constraintsType [i] = contactA.Type;
double mValue = addLCPValue(contactA,
contactA);
//Diagonal value
mValue += contactA.CFM +
SimulationEngineParameters.CFM +
1E-40;
D[i] = 1.0 / mValue;
for (int j = i + 1; j < contact.Length; j++)
{
JacobianContact contactB = contact[j];
if (contactA.ObjectA == contactB.ObjectA ||
contactA.ObjectB == contactB.ObjectB ||
contactA.ObjectA == contactB.ObjectB ||
contactA.ObjectB == contactB.ObjectA)
{
if (contactA.Type == contactB.Type &&
contactB.Type == ConstraintType.Collision &&
contactA.ObjectA == contactB.ObjectA &&
contactA.ObjectB == contactB.ObjectB)
{
constraints[i].Add(j);
constraints[j].Add(i);
}
mValue = addLCPValue(
contactA,
contactB);
if (Math.Abs(mValue) > 1E-30)
{
lock (sync)
{
index[i].Add(j);
value[i].Add(mValue);
index[j].Add(i);
value[j].Add(mValue);
}
}
}
}
});
int?[][] constraintsArray = new int?[contact.Length][];
for (int i = 0; i < contact.Length; i++)
{
M [i] = new SparseElement (
value [i].ToArray (),
index [i].ToArray (),
contact.Length);
constraintsArray[i] = constraints[i].ToArray();
}
return new LinearProblemProperties (
M,
B,
X,
D,
constraintsLimit,
constraintsType,
constraintsArray,
contact.Length);
}
return null;
}
private void PhysicsExecutionFlow()
{
var stopwatch = new Stopwatch();
stopwatch.Reset();
stopwatch.Start();
#region Contact and Joint elaboration
solverError = 0.0;
if (SimulationEngineParameters.PositionStabilization)
{
bool positionUpdated = PhysicsJointPositionCorrection();
if (positionUpdated)
{
CollisionDetectionStep();
PartitionEngineExecute();
}
}
if (collisionPartitionedPoints != null)
{
bool convertSetting = false;
if (SimulationEngineParameters.PositionStabilization)
{
SimulationEngineParameters.SetPositionStabilization(false);
convertSetting = true;
}
for (int i = 0; i < collisionPartitionedPoints.Count;i++)
{
JacobianContact[] jacobianConstraints = GetJacobianConstraint(
collisionPartitionedPoints[i].ToArray(),
partitionedJoint[i],
simulationObjects,
SimulationEngineParameters).ToArray();
if (jacobianConstraints.Length > 0)
{
SolutionValues[] overallSolution = new SolutionValues[jacobianConstraints.Length];
#region Solve Normal And Friction Constraints
if (SimulationEngineParameters.FrictionAndNormalIterations > 0)
{
JacobianContact[] frictionConstraint = Helper.FilterConstraints(jacobianConstraints,
ConstraintType.Friction,
ConstraintType.Collision);
LinearProblemProperties frictionLCP = BuildLCPMatrix(
frictionConstraint,
SimulationEngineParameters.PositionStabilization);
SolutionValues[] contactSolution = BuildMatrixAndExecuteSolver(
frictionConstraint,
frictionLCP,
SimulationEngineParameters.FrictionAndNormalIterations);
}
#endregion
#region Solve Joint Constraint
if (simulationJoints.Count > 0 &&
SimulationEngineParameters.JointsIterations > 0)
{
JacobianContact[] jointConstraints = Helper.FindJointConstraints(jacobianConstraints);
LinearProblemProperties jointLCP = BuildLCPMatrix(
jointConstraints,
SimulationEngineParameters.PositionStabilization);
SolutionValues[] jointSolution = BuildMatrixAndExecuteSolver(
jointConstraints,
jointLCP,
SimulationEngineParameters.JointsIterations);
}
#endregion
#region Solver Overall Constraints
jacobianConstraints = ContactSorting(jacobianConstraints);
LinearProblemProperties overallLCP = BuildLCPMatrix(
jacobianConstraints,
SimulationEngineParameters.PositionStabilization);
if (overallLCP != null &&
SimulationEngineParameters.OverallConstraintsIterations > 0)
{
solver.GetSolverParameters().SetSolverMaxIteration(SimulationEngineParameters.OverallConstraintsIterations);
overallSolution = solver.Solve(overallLCP);
double[] overallError = new double[overallLCP.Count];
//SolverParameters test = new SolverParameters(300, solver.GetSolverParameters().ErrorTolerance, solver.GetSolverParameters().SOR, solver.GetSolverParameters().MaxThreadNumber, solver.GetSolverParameters().SORStep, solver.GetSolverParameters().DynamicSORUpdate);
//ProjectedGaussSeidel testVerifica = new ProjectedGaussSeidel(test);
//SolutionValues[] sol = testVerifica.Solve(overallLCP);
Console.WriteLine("error " + SolverHelper.ComputeSolverError(overallLCP, overallSolution));
//Console.WriteLine("errorTest " + SolverHelper.ComputeSolverError(overallLCP, sol));
}
else if (SimulationEngineParameters.OverallConstraintsIterations == 0)
{
for (int j = 0; j < overallSolution.Length; j++)
overallSolution[j].X = jacobianConstraints[j].StartImpulse.StartImpulseValue;
}
UpdateVelocity(
jacobianConstraints,
simulationObjects,
overallSolution);
#endregion
}
}
if (convertSetting)
{
SimulationEngineParameters.SetPositionStabilization(true);
}
}
#endregion
#region Position and Velocity integration
IntegrateObjectsPosition (simulationObjects);
#endregion
stopwatch.Stop();
Console.WriteLine("Inner Engine Elapsed={0}", stopwatch.ElapsedMilliseconds);
}
private void UpdatePositionBasedVelocity(
JacobianContact[] contact,
SimulationObject[] simulationObj,
SolutionValues[] X)
{
for (int i = 0; i < contact.Length; i++)
{
double impulse = X[i].X;
JacobianContact ct = contact[i];
SetPositionBasedVelocity(
simulationObj,
ct.LinearComponentA,
ct.AngularComponentA,
impulse,
ct.ObjectA);
SetPositionBasedVelocity(
simulationObj,
ct.LinearComponentB,
ct.AngularComponentB,
impulse,
ct.ObjectB);
}
}
/// <summary>
/// Updates velocity of the simulations objects.
/// </summary>
private void UpdateVelocity(
JacobianContact[] contact,
SimulationObject[] simulationObj,
SolutionValues[] X)
{
for (int i =0; i< contact.Length;i++)
{
if (Math.Abs(X[i].X) > 1E-50)
{
double impulse = X[i].X;
JacobianContact ct = contact[i];
UpdateObjectVelocity(
simulationObj,
ct.LinearComponentA,
ct.AngularComponentA,
impulse,
ct.ObjectA);
UpdateObjectVelocity(
simulationObj,
ct.LinearComponentB,
ct.AngularComponentB,
impulse,
ct.ObjectB);
ct.StartImpulse.SetStartValue(impulse * SimulationEngineParameters.WarmStartingValue);
}
}
}
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 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;
}
