private void AddLCPValues(
ref List <int> index,
ref List <double> values,
int indexVal,
KeyValuePair <HashSetStruct, List <DictionaryConstraintValue> > constraintCheckItem,
HashSetStruct constraintValuesKey,
HashSetStruct symmetricHashSet,
Vector3d?linearComponentA,
Vector3d angularComponentA,
MassData massData,
bool componentA)
{
double mValue = 0.0;
if (!constraintCheckItem.Key.Equals(constraintValuesKey) &&
!constraintCheckItem.Key.Equals(symmetricHashSet))
{
for (int i = 0; i < constraintCheckItem.Value.Count; i++)
{
int innerIndex = constraintCheckItem.Value[i].Index;
JacobianConstraint contactB = constraintCheckItem.Value[i].Constraint;
var lComponent = contactB.LinearComponentB;
var aComponent = contactB.AngularComponentB;
if (componentA)
{
lComponent = contactB.LinearComponentA;
aComponent = contactB.AngularComponentA;
}
mValue = GetLCPMatrixValue(
linearComponentA,
lComponent,
angularComponentA,
aComponent,
massData);
AddValues(ref index, ref values, mValue, innerIndex);
}
}
}
public LinearProblemProperties BuildLCP(
JacobianConstraint[] constraints,
double timeStep)
{
LinearProblemBaseProperties baseProperties = new LinearProblemBaseProperties(constraints.Length);
Dictionary <HashSetStruct, List <DictionaryConstraintValue> > constraintsDictionary = new Dictionary <HashSetStruct, List <DictionaryConstraintValue> >();
for (int i = 0; i < constraints.Length; i++)
{
JacobianConstraint itemConstraint = constraints[i];
HashSetStruct hash = new HashSetStruct(itemConstraint.ObjectA.ID, itemConstraint.ObjectB.ID);
if (constraintsDictionary.TryGetValue(hash, out List <DictionaryConstraintValue> jc))
{
jc.Add(new DictionaryConstraintValue(itemConstraint, i));
}
else
{
constraintsDictionary.Add(hash, new List <DictionaryConstraintValue> {
new DictionaryConstraintValue(itemConstraint, i)
});
}
}
Graph graph = new Graph(constraints.Length);
var dictionaryArray = constraintsDictionary.ToArray();
var key_ID_A = constraintsDictionary.ToLookup(x => x.Key.ID_A);
var key_ID_B = constraintsDictionary.ToLookup(x => x.Key.ID_B);
var rangePartitioner = Partitioner.Create(
0,
dictionaryArray.Length,
Convert.ToInt32(dictionaryArray.Length / EngineParameters.MaxThreadNumber) + 1);
double timeFreq = 1.0 / timeStep;
Parallel.ForEach(
rangePartitioner,
new ParallelOptions {
MaxDegreeOfParallelism = EngineParameters.MaxThreadNumber
},
(range, loopState) =>
{
for (int i = range.Item1; i < range.Item2; i++)
{
var constraintValues = dictionaryArray[i];
var constraintValueKey = constraintValues.Key;
int contactA_ID_A = constraintValues.Key.ID_A;
int contactA_ID_B = constraintValues.Key.ID_B;
for (int w = 0; w < constraintValues.Value.Count; w++)
{
JacobianConstraint contactA = constraintValues.Value[w].Constraint;
List <int> index = new List <int>();
List <double> values = new List <double>();
int indexVal = constraintValues.Value[w].Index;
double correctionVal = contactA.CorrectionValue * timeFreq;
double correctionValue = (correctionVal) < 0 ?
Math.Max(correctionVal, -EngineParameters.MaxCorrectionValue) :
Math.Min(correctionVal, EngineParameters.MaxCorrectionValue);
baseProperties.B[indexVal] = correctionValue - contactA.JacobianVelocity - contactA.ConstraintValue;
baseProperties.ConstraintsArray[indexVal] = contactA.ContactReference;
baseProperties.ConstraintLimit[indexVal] = contactA.ConstraintLimit;
baseProperties.ConstraintType[indexVal] = contactA.Type;
//Diagonal value
double mValue = GetLCPDiagonalValue(contactA) +
(contactA.CFM * timeFreq) +
EngineParameters.CFM +
1E-40;
baseProperties.D[indexVal] = mValue;
baseProperties.InvD[indexVal] = 1.0 / mValue;
//contactA_ID_A == contactB_ID_A && contactA_ID_B == contactB_ID_B
for (int j = 0; j < constraintValues.Value.Count; j++)
{
int innerIndex = constraintValues.Value[j].Index;
if (innerIndex != indexVal)
{
JacobianConstraint contactB = constraintValues.Value[j].Constraint;
mValue = GetLCPMatrixValue(
contactA.LinearComponentA,
contactB.LinearComponentA,
contactA.AngularComponentA,
contactB.AngularComponentA,
contactA.ObjectA.MassInfo);
mValue += GetLCPMatrixValue(
contactA.LinearComponentB,
contactB.LinearComponentB,
contactA.AngularComponentB,
contactB.AngularComponentB,
contactA.ObjectB.MassInfo);
AddValues(ref index, ref values, mValue, innerIndex);
}
}
//contactA_ID_A == contactB_ID_B && contactA_ID_B == contactB_ID_A
var symmetricHashSet = new HashSetStruct(contactA_ID_B, contactA_ID_A);
if (constraintsDictionary.TryGetValue(symmetricHashSet, out List <DictionaryConstraintValue> symmetricList))
{
foreach (var item in symmetricList)
{
int innerIndex = item.Index;
if (innerIndex != indexVal)
{
JacobianConstraint contactB = item.Constraint;
mValue = GetLCPMatrixValue(
contactA.LinearComponentA,
contactB.LinearComponentB,
contactA.AngularComponentA,
contactB.AngularComponentB,
contactA.ObjectA.MassInfo);
mValue += GetLCPMatrixValue(
contactA.LinearComponentB,
contactB.LinearComponentA,
contactA.AngularComponentB,
contactB.AngularComponentA,
contactA.ObjectB.MassInfo);
AddValues(ref index, ref values, mValue, innerIndex);
}
}
}
//contactA_ID_A == contactB_ID_A
foreach (var constraintCheckItem in key_ID_A[contactA_ID_A])
{
AddLCPValues(
ref index,
ref values,
indexVal,
constraintCheckItem,
constraintValueKey,
symmetricHashSet,
contactA.LinearComponentA,
contactA.AngularComponentA,
contactA.ObjectA.MassInfo,
true);
}
//contactA_ID_A == contactB_ID_B
foreach (var constraintCheckItem in key_ID_B[contactA_ID_A])
{
AddLCPValues(
ref index,
ref values,
indexVal,
constraintCheckItem,
constraintValueKey,
symmetricHashSet,
contactA.LinearComponentA,
contactA.AngularComponentA,
contactA.ObjectA.MassInfo,
false);
}
//contactA_ID_B == contactB_ID_A
foreach (var constraintCheckItem in key_ID_A[contactA_ID_B])
{
AddLCPValues(
ref index,
ref values,
indexVal,
constraintCheckItem,
constraintValueKey,
symmetricHashSet,
contactA.LinearComponentB,
contactA.AngularComponentB,
contactA.ObjectB.MassInfo,
true);
}
//contactA_ID_B == contactB_ID_B
foreach (var constraintCheckItem in key_ID_B[contactA_ID_B])
{
AddLCPValues(
ref index,
ref values,
indexVal,
constraintCheckItem,
constraintValueKey,
symmetricHashSet,
contactA.LinearComponentB,
contactA.AngularComponentB,
contactA.ObjectB.MassInfo,
false);
}
//Sparse Matrix
baseProperties.M.Rows[indexVal] = new SparseVector(
values.ToArray(),
index.ToArray(),
baseProperties.M.m);
UpdateGraph(ref graph, index, indexVal);
}
}
});
var lp = new LinearProblemProperties(
baseProperties,
graph,
EngineParameters.FrictionDirections);
return(lp);
}
