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; }