private double[] GetPhi( LinearProblemProperties input, SparseElement[] A, double[] x, double[] g) { double[] result = new double[input.Count]; for (int i = 0; i < input.Count; i++) { if (!ClampSolution.GetIfClamped(input, x, i)) result[i] = g[i]; else { double min = 0.0; double max = 0.0; ClampSolution.GetConstraintValues(input, x, i, ref min, ref max); if (x[i] >= min && x[i] <= max) { result[i] = g[i]; } else { result[i] = 0.0; } } } return result; }
public LinearProblemProperties ( SparseElement[] M, double[] B, SolutionValues[] startX, double[] d, double[] constraintLimit, ConstraintType[] constraintType, int?[][] constraints, int count) { this.M = M; this.B = B; D = d; ConstraintLimit = constraintLimit; ConstraintType = constraintType; Constraints = constraints; Count = count; }
public static double[] Multiply(SparseElement[] matrix, double[] vector) { if(matrix[0].RowLength != vector.Length) throw new Exception("Wrong input length"); double[] result = new double[matrix.Length]; for (int i = 0; i < matrix.Length; i++) { SparseElement m = matrix[i]; double[] bufValue = m.Value; int[] bufIndex = m.Index; double bValue = 0.0; for (int j = 0; j < m.Count; j++) bValue += bufValue[j] * vector[bufIndex[j]]; result[i] = bValue; } return result; }
/// <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; }
public SparseElement[] GetOriginalMatrixSparse() { SparseElement[] originalMatrix = new SparseElement[M.Length]; for (int i = 0; i < M.Length; i++) { List<double> valueList = M[i].Value.ToList(); List<int> indexList = M[i].Index.ToList(); valueList.Add(1.0 / D[i]); indexList.Add(i); originalMatrix[i] = new SparseElement(valueList.ToArray(), indexList.ToArray(), M[i].RowLength); } return originalMatrix; }