public DictionaryConstraintValue(
JacobianConstraint constraint,
int startIndex)
{
this.Constraint = constraint;
this.Index = startIndex;
}
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 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 GetLCPDiagonalValue(JacobianConstraint contact)
{
double LCPvalue = 0.0;
if (contact.LinearComponentA.HasValue)
{
LCPvalue += contact.LinearComponentA.Value.Dot(
contact.LinearComponentA.Value * contact.ObjectA.MassInfo.InverseMass);
}
LCPvalue += contact.AngularComponentA.Dot(
contact.ObjectA.MassInfo.InverseInertiaTensor * contact.AngularComponentA);
if (contact.LinearComponentB.HasValue)
{
LCPvalue += contact.LinearComponentB.Value.Dot(
contact.LinearComponentB.Value * contact.ObjectB.MassInfo.InverseMass);
}
LCPvalue += contact.AngularComponentB.Dot(
contact.ObjectB.MassInfo.InverseInertiaTensor * contact.AngularComponentB);
return(LCPvalue);
}
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);
}
/// <summary>
/// Update object velocity
/// </summary>
/// <param name="contact"></param>
/// <param name="X"></param>
public void UpdateVelocity(
JacobianConstraint[] contact,
double[] x)
{
var rangePartitioner = Partitioner.Create(0, contact.Length, Convert.ToInt32(contact.Length / EngineParameters.MaxThreadNumber) + 1);
//Critical section variable
var sync = new object();
Parallel.ForEach(
rangePartitioner,
new ParallelOptions {
MaxDegreeOfParallelism = EngineParameters.MaxThreadNumber
},
(range, loopState) =>
{
for (int i = range.Item1; i < range.Item2; i++)
{
if (Math.Abs(x[i]) > tolerance)
{
double impulse = x[i];
JacobianConstraint ct = contact[i];
if (ct.LinearComponentA.HasValue)
{
UpdateObjectVelocity(
ct.ObjectA,
ct.LinearComponentA.Value,
ct.AngularComponentA,
impulse,
sync);
}
else
{
UpdateObjectVelocity(
ct.ObjectA,
ct.AngularComponentA,
impulse,
sync);
}
if (ct.LinearComponentB.HasValue)
{
UpdateObjectVelocity(
ct.ObjectB,
ct.LinearComponentB.Value,
ct.AngularComponentB,
impulse,
sync);
}
else
{
UpdateObjectVelocity(
ct.ObjectB,
ct.AngularComponentB,
impulse,
sync);
}
}
}
});
}
