public static JacobianContact[] FindJointConstraints(JacobianContact[] list)
{
var result = new List<JacobianContact>();
foreach (JacobianContact jc in list)
{
if (jc.Type != ConstraintType.Friction &&
jc.Type != ConstraintType.Collision)
result.Add(jc);
}
return result.ToArray();
}
public static JacobianContact[] PruneConstraintsFromSoftJoint(
JacobianContact[] list)
{
var result = new List<JacobianContact>();
foreach (JacobianContact jc in list)
{
if (jc.Type != ConstraintType.SoftJoint)
result.Add(jc);
}
return result.ToArray();
}
public static JacobianContact[] FilterConstraints(
JacobianContact[] list,
ConstraintType typeA)
{
var result = new List<JacobianContact>();
foreach (JacobianContact jc in list)
{
if (jc.Type == typeA)
result.Add(jc);
}
return result.ToArray();
}
public static JacobianContact[] FindConstraintsWithError(
JacobianContact[] list,
ConstraintType typeA,
ConstraintType typeB)
{
var result = new List<JacobianContact>();
foreach (JacobianContact jc in list)
{
if ((jc.Type == typeA ||
jc.Type == typeB) &&
Math.Abs(jc.CorrectionValue) > 1E-100)
result.Add(jc);
}
return result.ToArray();
}
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;
}
/// <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 JacobianContact[] ContactSorting(JacobianContact[] jacobianContact)
{
var sorted = jacobianContact.Select((x, i) => new KeyValuePair<JacobianContact, int>(x, i)).
OrderBy(x => Math.Abs(x.Key.B)).ToArray();
int[] sortedIndex = sorted.Select(x => x.Value).ToArray();
JacobianContact[] sortedContact = sorted.Select(x => x.Key).ToArray();
int[] randomIndex = new int[jacobianContact.Length];
for (int i = 0; i < randomIndex.Length; i++)
randomIndex[sortedIndex[i]] = i;
for (int i = 0; i < sortedContact.Length; i++)
{
if (sortedContact[i].Type == ConstraintType.Friction)
sortedContact[i].SetContactReference(randomIndex[sortedContact[i].ContactReference.Value]);
}
return sortedContact;
}
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 addLCPValue(
JacobianContact contactA,
JacobianContact contactB)
{
double linearA = 0.0;
double angularA = 0.0;
if (contactA.ObjectA == contactB.ObjectA) {
linearA = contactA.LinearComponentA.Dot (
contactB.LinearComponentA * simulationObjects [contactA.ObjectA].InverseMass);
angularA = contactA.AngularComponentA.Dot (
simulationObjects [contactA.ObjectA].InertiaTensor * contactB.AngularComponentA);
} else if (contactB.ObjectB == contactA.ObjectA) {
linearA = contactA.LinearComponentA.Dot (
contactB.LinearComponentB * simulationObjects [contactA.ObjectA].InverseMass);
angularA = contactA.AngularComponentA.Dot (
simulationObjects [contactA.ObjectA].InertiaTensor * contactB.AngularComponentB);
}
double linearB = 0.0;
double angularB = 0.0;
if (contactB.ObjectA == contactA.ObjectB) {
linearB = contactA.LinearComponentB.Dot (
contactB.LinearComponentA * simulationObjects [contactA.ObjectB].InverseMass);
angularB = contactA.AngularComponentB.Dot(
simulationObjects [contactA.ObjectB].InertiaTensor * contactB.AngularComponentA);
} else if (contactB.ObjectB == contactA.ObjectB) {
linearB = contactA.LinearComponentB.Dot (
contactB.LinearComponentB * simulationObjects [contactA.ObjectB].InverseMass);
angularB = contactA.AngularComponentB.Dot (
simulationObjects [contactA.ObjectB].InertiaTensor * contactB.AngularComponentB);
}
return (linearA + angularA) +
(linearB + angularB);
}
private static JacobianContact[] addFriction(
SimulationObject objectA,
SimulationObject objectB,
SimulationParameters simulationParameters,
int indexA,
int indexB,
Vector3 normal,
Vector3 relativeVelocity,
Vector3 ra,
Vector3 rb,
List<StartImpulseProperties> startImpulseProperties)
{
JacobianContact[] friction = new JacobianContact[2];
var tx = new Vector3 ();
var ty = new Vector3 ();
GeometryUtilities.ComputeBasis(
normal,
ref tx,
ref ty);
double constraintLimit = 0.0;
Vector3 tangentialVelocity = relativeVelocity -
(normal.Dot(relativeVelocity)) *
normal;
#region Get start friction direction
if (Vector3.Length (tangentialVelocity) >
simulationParameters.ShiftToStaticFrictionTolerance)
constraintLimit = 0.5 * (objectA.DynamicFrictionCoeff + objectB.DynamicFrictionCoeff);
else
constraintLimit = 0.5 * (objectA.StaticFrictionCoeff + objectB.StaticFrictionCoeff);
#endregion
#region Tangential Direction 1
var linearComponentA = tx;
var linearComponentB = -1.0 * linearComponentA;
var angularComponentA = ra.Cross (linearComponentA);
var angularComponentB = -1.0 * rb.Cross (linearComponentA);
friction [0] = JacobianCommon.GetDOF (
indexA,
indexB,
linearComponentA,
linearComponentB,
angularComponentA,
angularComponentB,
objectA,
objectB,
0.0,
0.0,
simulationParameters.FrictionCFM,
constraintLimit,
ConstraintType.Friction,
null,
startImpulseProperties[1]);
#endregion
#region Tangential Direction 2
linearComponentA = ty;
linearComponentB = -1.0 * linearComponentA;
angularComponentA = ra.Cross (linearComponentA);
angularComponentB = -1.0 * rb.Cross (linearComponentA);
friction [1] = JacobianCommon.GetDOF (
indexA,
indexB,
linearComponentA,
linearComponentB,
angularComponentA,
angularComponentB,
objectA,
objectB,
0.0,
0.0,
simulationParameters.FrictionCFM,
constraintLimit,
ConstraintType.Friction,
null,
startImpulseProperties[2]);
#endregion
return friction;
}