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 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 static double[] Multiply(SparseElement[] matrix, double[] vector)
{
if(matrix[0].RowLength != vector.Length)
throw new Exception("Wrong input length");
double[] result = new double[matrix.Length];
for (int i = 0; i < matrix.Length; i++)
{
SparseElement m = matrix[i];
double[] bufValue = m.Value;
int[] bufIndex = m.Index;
double bValue = 0.0;
for (int j = 0; j < m.Count; j++)
bValue += bufValue[j] * vector[bufIndex[j]];
result[i] = bValue;
}
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;
}
public SparseElement[] GetOriginalMatrixSparse()
{
SparseElement[] originalMatrix = new SparseElement[M.Length];
for (int i = 0; i < M.Length; i++)
{
List<double> valueList = M[i].Value.ToList();
List<int> indexList = M[i].Index.ToList();
valueList.Add(1.0 / D[i]);
indexList.Add(i);
originalMatrix[i] = new SparseElement(valueList.ToArray(), indexList.ToArray(), M[i].RowLength);
}
return originalMatrix;
}
