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