public double[] SolveExecute( LinearProblemProperties input, Dictionary <RedBlackEnum, List <int> > redBlackDictionary, double[] x) { return(Execute(input, redBlackDictionary, x)); }
private double[] Execute( LinearProblemProperties input, double[] x, int[] solverOrder) { double[] oldX = new double[x.Length]; double[] result = new double[x.Length]; Array.Copy(x, result, x.Length); double actualSolverError = 0.0; for (int k = 0; k < SolverParameters.MaxIterations; k++) { var sum = ElaborateLowerTriangularMatrix(input, result); actualSolverError = ElaborateUpperTriangularMatrix( input, sum, solverOrder, ref result, ref oldX); if (actualSolverError < SolverParameters.ErrorTolerance) { break; } } return(result); }
public Dictionary <RedBlackEnum, List <int> > GetRedBlackDictionary(LinearProblemProperties input) { var graph = input.ConstrGraph; var nodeDictionary = BreadthFirstSearch.GetBoolLevelBFS(graph, 0); if (nodeDictionary.Count < input.Count) { for (int i = 0; i < input.Count; i++) { if (!nodeDictionary.ContainsKey(i)) { var dict = BreadthFirstSearch.GetBoolLevelBFS(graph, i); foreach (var element in dict) { nodeDictionary.Add(element.Key, element.Value); } if (nodeDictionary.Count == input.Count) { break; } } } } var redBlackDictionary = GetRedBlackDictionary(nodeDictionary); return(redBlackDictionary); }
static double Kernel( LinearProblemProperties input, double[] x, int i) { double sumBuffer = 0.0; //Avoid last row elaboration if (i + 1 != input.Count && input.M.Rows[i].Index.Length > 0) { var bufValue = input.M.Rows[i].Value; var bufIndex = input.M.Rows[i].Index; for (int j = 0; j < bufIndex.Length; j++) { int idx = bufIndex[j]; if (idx > i) { sumBuffer += bufValue[j] * x[idx]; } } } return(sumBuffer); }
public double[] Solve( LinearProblemProperties input, double[] x, int[] solverOrder) { return(Execute(input, x, solverOrder)); }
public double[] Solve( LinearProblemProperties input, JacobianConstraint[] constraints, double[] x) { double[] oldX = new double[x.Length]; double[] result = new double[x.Length]; Array.Copy(x, result, x.Length); double actualSolverError = 0.0; result = gaussSeidelSolver.Solve(input, constraints, oldX); for (int i = 0; i < solverParam.MaxIterations; i++) { velocityIntegration.UpdateVelocity(constraints, result); input = lcpEngine.UpdateConstantsTerm(constraints, input); result = gaussSeidelSolver.Solve(input, constraints, new double[x.Length]); actualSolverError = SolverHelper.ComputeSolverError(result, oldX); if (actualSolverError < solverParam.ErrorTolerance) { break; } Array.Copy(result, oldX, x.Length); } return(result); }
public static double ComputeSolverError( LinearProblemProperties input, double[] x) { double error = 0.0; var counter = input.ConstraintType.Count(s => s != ShapeDefinition.ConstraintType.Friction); if (counter > 0) { for (int i = 0; i < input.Count; i++) { SparseVector m = input.M.Rows[i]; if (input.ConstraintType[i] != ShapeDefinition.ConstraintType.Friction) { var bufValue = m.Value; var bufIndex = m.Index; double bValue = (1.0 / input.InvD[i]) * x[i]; for (int j = 0; j < bufIndex.Length; j++) { bValue += bufValue[j] * x[bufIndex[j]]; } error += (bValue - input.B[i]) * (bValue - input.B[i]); } } return(error / counter); } return(-1.0); }
public double[] Solve( LinearProblemProperties input, JacobianConstraint[] constraints, double[] x) { var solverOrder = Enumerable.Range(0, x.Length).ToArray(); return(Execute(input, x, solverOrder)); }
public double[] Solve( LinearProblemProperties input, JacobianConstraint[] constraints, double[] x) { var redBlackDictionary = GetRedBlackDictionary(input); return(Execute(input, redBlackDictionary, x)); }
private double[] ElaborateLowerTriangularMatrix( LinearProblemProperties input, double[] x) { double[] sum = new double[input.Count]; for (int i = 0; i < input.Count; i++) { sum[i] = Kernel(input, x, i); } return(sum); }
public double[] Solve( LinearProblemProperties input, JacobianConstraint[] constraints, double[] x) { double[] Xk = gaussSeidelSolver.Solve(input, constraints, x); double[] delta = CalculateDelta(Xk, x); double[] searchDirection = NegateArray(delta); double[] Xk1 = new double[input.Count]; double modDeltak = 0.0; double oldModDeltak = 0.0; for (int i = 0; i < solverParam.MaxIterations; i++) { Xk1 = gaussSeidelSolver.Solve(input, constraints, Xk); double[] deltaK = CalculateDelta(Xk1, Xk); modDeltak = ArraySquareModule(deltaK); if (modDeltak < solverParam.ErrorTolerance) { break; } double betaK = (modDeltak * modDeltak) / (oldModDeltak * oldModDeltak); if (betaK > 1.0) { searchDirection = new double[searchDirection.Length]; } else { Xk1 = CalculateDirection( input, Xk1, deltaK, ref searchDirection, betaK); } Array.Copy(Xk1, Xk, Xk1.Length); oldModDeltak = modDeltak; } deltaErrorCheck = modDeltak; return(Xk1); }
private void UpdateFrictionConstraints(LinearProblemProperties lcp) { var frictionRes = new int?[lcp.Count]; for (int i = 0; i < lcp.Constraints.Length; i++) { if (lcp.ConstraintType[i] == ConstraintType.Collision) { for (int j = 0; j < lcp.FrictionDirections; j++) { frictionRes[i - j - 1] = -i; } } } lcp.SetConstraints(frictionRes); }
public static bool GetIfClamped( LinearProblemProperties input, double[] X, int i) { switch (input.ConstraintType[i]) { case ConstraintType.Collision: case ConstraintType.JointLimit: return(true); case ConstraintType.Friction: return(true); case ConstraintType.JointMotor: return(true); default: return(false); } }
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[] CalculateDirection( LinearProblemProperties input, double[] Xk1, double[] deltaK, ref double[] searchDirection, double betak) { double[] result = new double[Xk1.Length]; for (int i = 0; i < Xk1.Length; i++) { double bDirection = betak * searchDirection[i]; result[i] = Xk1[i] + bDirection; searchDirection[i] = bDirection - deltaK[i]; result[i] = ClampSolution.Clamp(input, result[i], ref result, i); } return(result); }
public double[] Solve( LinearProblemProperties linearProblemProperties, JacobianConstraint[] constraints, double[] x) { double lambda = 1.0; var A = linearProblemProperties.GetOriginalSparseMatrix(); var b = linearProblemProperties.B; int N = b.Length; double gamma = 1E-28; double error = double.PositiveInfinity; double oldError = 0.0; for (int iter = 0; iter < SolverParameters.MaxIterations; iter++) { double[] y = Add(SparseMatrix.Multiply(A, x, SolverParameters.MaxThreadNumber), b); double[] phi = PhiLambda(y, x, lambda); oldError = error; error = 0.5 * Dot(phi, phi); var fnd = Find(phi, x, gamma); List <int> S = fnd.Item1; List <int> I = fnd.Item2; int restart = Math.Min(A.n, 10); double[] nablaPhi = null; //TODO test other function var dx = RandomFunc(A, x, y, phi, I, N, restart, ref nablaPhi); x = ArmijoBacktracking(A, b, x, dx, nablaPhi, lambda, gamma, error); } return(x); }
private void ExecuteKernel( LinearProblemProperties input, int index, ref double[] x, object sync) { SparseVector m = input.M.Rows[index]; double xValue = x[index]; double sumBuffer = 0.0; var bufValue = m.Value; var bufIndex = m.Index; for (int j = 0; j < bufIndex.Length; j++) { int idx = bufIndex[j]; sumBuffer += bufValue[j] * x[idx]; } sumBuffer = (input.B[index] - sumBuffer) * input.InvD[index]; double sor = SolverParameters.SOR; // Constraints with limit diverge with sor > 1.0 if (sor > 1.0 && (input.ConstraintType[index] == ConstraintType.Friction || input.ConstraintType[index] == ConstraintType.JointMotor || input.ConstraintType[index] == ConstraintType.JointLimit)) { sor = 1.0; } xValue += (sumBuffer - xValue) * sor; x[index] = SetValue(input, xValue, ref x, index, sync); }
private double SetValue( LinearProblemProperties input, double value, ref double[] x, int i, object sync) { lock (sync) { switch (input.ConstraintType[i]) { case ConstraintType.Collision: case ConstraintType.JointLimit: return((value < 0.0) ? 0.0 : value); case ConstraintType.Friction: //Isotropic friction -> sqrt(c1^2+c2^2) <= fn*U int normalIndex = input.Constraints[i].Value; double frictionLimit = x[normalIndex] * input.ConstraintLimit[i]; if (frictionLimit == 0.0) { x[normalIndex + 1] = 0.0; x[normalIndex + 2] = 0.0; return(0.0); } double directionA = x[normalIndex + 1]; double directionB = x[normalIndex + 2]; double frictionValue = Math.Sqrt(directionA * directionA + directionB * directionB); if (frictionValue > frictionLimit) { Vector2d frictionNormal = new Vector2d(directionA / frictionValue, directionB / frictionValue); x[normalIndex + 1] = frictionNormal.x * frictionLimit; x[normalIndex + 2] = frictionNormal.y * frictionLimit; return((i - normalIndex == 1) ? x[normalIndex + 1] : x[normalIndex + 2]); } return(value); case ConstraintType.JointMotor: double limit = input.ConstraintLimit[i]; if (value < -limit) { return(-limit); } if (value > limit) { return(limit); } return(value); default: return(value); } } }
private double ElaborateUpperTriangularMatrix( LinearProblemProperties input, double[] sum, int[] solverOrder, ref double[] x, ref double[] oldX) { double error = 0.0; for (int i = 0; i < input.Count; i++) { int index = solverOrder[i]; double sumBuffer = sum[index]; SparseVector m = input.M.Rows[index]; ShapeDefinition.ConstraintType cType = input.ConstraintType[index]; double xValue = x[index]; //Avoid first row elaboration if (index != 0 && m.Index.Length > 0) { var bufValue = m.Value; var bufIndex = m.Index; for (int j = 0; j < bufIndex.Length; j++) { int idx = bufIndex[j]; if (idx < index) { sumBuffer += bufValue[j] * x[idx]; } } } sumBuffer = (input.B[index] - sumBuffer) * input.InvD[index]; double sor = SolverParameters.SOR; // Constraints with limit diverge with sor > 1.0 if (sor > 1.0 && (cType == ShapeDefinition.ConstraintType.Friction || cType == ShapeDefinition.ConstraintType.JointMotor || cType == ShapeDefinition.ConstraintType.JointLimit)) { sor = 1.0; } xValue += (sumBuffer - xValue) * sor; x[index] = ClampSolution.Clamp(input, xValue, ref x, index); //Compute error double diff = x[index] - oldX[index]; error += diff * diff; oldX[index] = x[index]; } return(error); }
public static double Clamp( LinearProblemProperties input, double xValue, ref double[] X, int i) { switch (input.ConstraintType[i]) { case ConstraintType.Collision: case ConstraintType.JointLimit: return((xValue < 0.0) ? 0.0: xValue); case ConstraintType.Friction: //Isotropic friction -> sqrt(c1^2+c2^2) <= fn*U int sign = (input.Constraints[i].Value < 0) ? -1 : 1; int normalIndex = sign * input.Constraints[i].Value; double frictionLimit = X[normalIndex] * input.ConstraintLimit[i]; int idx1 = normalIndex + sign; int idx2 = normalIndex + sign * 2; if (frictionLimit == 0.0) { X[idx1] = 0.0; X[idx2] = 0.0; return(0.0); } double directionA = X[idx1]; double directionB = X[idx2]; double frictionValue = Math.Sqrt(directionA * directionA + directionB * directionB); if (frictionValue > frictionLimit) { Vector2d frictionNormal = new Vector2d(directionA / frictionValue, directionB / frictionValue); X[idx1] = frictionNormal.x * frictionLimit; X[idx2] = frictionNormal.y * frictionLimit; return((Math.Abs(i - normalIndex) == 1) ? X[idx1] : X[idx2]); } return(xValue); case ConstraintType.JointMotor: double limit = input.ConstraintLimit[i]; if (xValue < -limit) { return(-limit); } if (xValue > limit) { return(limit); } return(xValue); default: return(xValue); } }
private double[] Execute( LinearProblemProperties input, Dictionary <RedBlackEnum, List <int> > redBlackDictionary, double[] x) { double[] oldX = new double[x.Length]; double[] result = new double[x.Length]; Array.Copy(x, result, x.Length); double actualSolverError = 0.0; redBlackDictionary.TryGetValue(RedBlackEnum.Red, out List <int> red); redBlackDictionary.TryGetValue(RedBlackEnum.Black, out List <int> black); OrderablePartitioner <Tuple <int, int> > rangePartitionerBlack = null; OrderablePartitioner <Tuple <int, int> > rangePartitionerRed = null; try { if (black.Any()) { rangePartitionerBlack = Partitioner.Create(0, black.Count, Convert.ToInt32(black.Count / SolverParameters.MaxThreadNumber) + 1); } if (red.Any()) { rangePartitionerRed = Partitioner.Create(0, red.Count, Convert.ToInt32(red.Count / SolverParameters.MaxThreadNumber) + 1); } for (int k = 0; k < SolverParameters.MaxIterations; k++) { var sync = new object(); if (red.Any()) { //Execute Red Parallel.ForEach( rangePartitionerRed, new ParallelOptions { MaxDegreeOfParallelism = SolverParameters.MaxThreadNumber }, (range, loopState) => { for (int i = range.Item1; i < range.Item2; i++) { ExecuteKernel(input, red[i], ref result, sync); } }); } if (black.Any()) { //Execute Black Parallel.ForEach( rangePartitionerBlack, new ParallelOptions { MaxDegreeOfParallelism = SolverParameters.MaxThreadNumber }, (range, loopState) => { for (int i = range.Item1; i < range.Item2; i++) { ExecuteKernel(input, black[i], ref result, sync); } }); } actualSolverError = SolverHelper.ComputeSolverError(result, oldX); if (actualSolverError < SolverParameters.ErrorTolerance) { break; } Array.Copy(result, oldX, x.Length); } } catch (Exception ex) { throw new Exception(ex.Message); } return(result); }