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 static double ComputeSolverError( LinearProblemProperties input, SolutionValues[] X) { double error = 0.0; for (int i = 0; i < input.Count; i++) { if (X[i].ConstraintStatus) continue; SparseElement m = input.M[i]; double[] bufValue = m.Value; int[] bufIndex = m.Index; double bValue = (1.0 / input.D[i]) * X[i].X; for (int j = 0; j < m.Count; j++) bValue += bufValue[j] * X[bufIndex[j]].X; error += (bValue - input.B[i]) * (bValue - input.B[i]); } return error; }
public SolutionValues[] Solve( LinearProblemProperties input, SolutionValues[] X = null) { if(X == null) X = new SolutionValues[input.Count]; double internalSOR = SolverParameters.SOR; double solverError = double.MaxValue; for (int k = 0; k < SolverParameters.MaxIteration; k++) { double[] sum = ElaborateLowerTriangularMatrix(input, X); for (int i = 0; i < input.Count; i++) { double sumBuffer = sum [i]; SparseElement m = input.M [i]; double[] bufValue = m.Value; int[] bufIndex = m.Index; //Avoid first row elaboration if (i != 0) { for (int j = 0; j < m.Count; j++) { if (bufIndex[j] < i) sumBuffer += bufValue [j] * X [bufIndex[j]].X; } } sumBuffer = (input.B[i] - sumBuffer) * input.D[i]; X[i].X += (sumBuffer - X[i].X) * internalSOR; X[i] = ClampSolution.Clamp (input, X, i); sum[i] = sumBuffer; } if (SolverParameters.DynamicSORUpdate) { double actualSolverError = SolverHelper.ComputeSolverError(input, X); if (actualSolverError > solverError) internalSOR = Math.Max(internalSOR - SolverParameters.SORStep, SolverParameters.SORStep); else solverError = actualSolverError; if (actualSolverError < SolverParameters.ErrorTolerance) return X; } } return X; }
public SolutionValues[] Solve( LinearProblemProperties linearProblemProperties, SolutionValues[] X = null) { if (X == null) X = new SolutionValues[linearProblemProperties.Count]; double[] x = new double[X.Length]; for (int i = 0; i < x.Length; i++) x[i] = X[i].X; SparseElement[] A = linearProblemProperties.GetOriginalMatrixSparse(); double[] g = Minus(Multiply(A, x), linearProblemProperties.B); if (Dot(g, g) < 1E-50) return X; double[] pPhi = GetPhi(linearProblemProperties, A, x, g); for (int i = 0; i < SolverParameters.MaxIteration; i++) { double alphaKDen = Dot(Multiply(A, pPhi), pPhi); double alphaK = 0.0; if (alphaKDen != 0) alphaK = Dot(g, pPhi) / alphaKDen; else { string ciao = "ciao"; } double[] halfX = Minus(x, Multiply(alphaK, pPhi)); x = Project(linearProblemProperties, halfX); g = Minus(Multiply(A, x), linearProblemProperties.B); pPhi = GetPhi(linearProblemProperties, A, x, g); } for (int i = 0; i < x.Length; i++) { X[i].X = x[i]; X[i] = ClampSolution.Clamp(linearProblemProperties, X, i); } return X; }
public SolutionValues[] Solve( LinearProblemProperties input, SolutionValues[] X = null) { if (X == null) X = new SolutionValues[input.Count]; SolutionValues[] Xk = gaussSeidelSolver.Solve(input); double[] delta = calculateDelta(Xk, X); double[] searchDirection = negateArray(delta); SolutionValues[] Xk1 = new SolutionValues[input.Count]; for (int i = 0; i < solverParam.MaxIteration; i++) { Xk1 = gaussSeidelSolver.Solve(input, Xk); double[] deltaK = calculateDelta(Xk1, Xk); deltaErrorCheck = arraySquareModule(delta); double betaK = 1.1; if (Math.Abs(deltaErrorCheck) > 1E-40) betaK = arraySquareModule(deltaK) / deltaErrorCheck; if (betaK > 1.0) searchDirection = new double[searchDirection.Length]; else { Xk1 = calculateDirection ( input, Xk1, deltaK, ref searchDirection, betaK); } Array.Copy(Xk1, Xk, Xk1.Length); Array.Copy(deltaK, delta, deltaK.Length); } return Xk1; }
public static SolutionValues Clamp( LinearProblemProperties input, SolutionValues[] X, int i) { switch (input.ConstraintType[i]) { case ConstraintType.Collision: case ConstraintType.JointLimit: if (X[i].X < 0) return new SolutionValues(0.0, true); else return new SolutionValues(X[i].X, false); case ConstraintType.Friction: double frictionLimit = X[input.Constraints[i][0].Value].X * input.ConstraintLimit[i]; if (X[i].X < -frictionLimit) return new SolutionValues(-frictionLimit, true); if (X[i].X > frictionLimit) return new SolutionValues(frictionLimit, true); return new SolutionValues(X[i].X, false); case ConstraintType.JointMotor: double limit = input.ConstraintLimit[i]; if (X[i].X < -limit) return new SolutionValues(-limit, true); if (X[i].X > limit) return new SolutionValues(limit, true); return new SolutionValues(X[i].X, false); default: return new SolutionValues(X[i].X, false); } }
public static double Clamp( LinearProblemProperties input, double[] X, int i) { switch (input.ConstraintType[i]) { case ConstraintType.Collision: case ConstraintType.JointLimit: if (X[i] < 0) return 0.0; else return X[i]; case ConstraintType.Friction: double frictionLimit = X[input.Constraints[i][0].Value] * input.ConstraintLimit[i]; if (X[i] < -frictionLimit) return -frictionLimit; if (X[i] > frictionLimit) return frictionLimit; return X[i]; case ConstraintType.JointMotor: double limit = input.ConstraintLimit[i]; if (X[i] < -limit) return -limit; if (X[i] > limit) return limit; return X[i]; default: return X[i]; } }
public static bool GetIfClamped( LinearProblemProperties input, double[] X, int i) { switch (input.ConstraintType[i]) { case ConstraintType.Collision: case ConstraintType.JointLimit: if (Math.Abs(X[i]) < 1E-50) return true; else return false; case ConstraintType.Friction: double frictionLimit = X[input.Constraints[i][0].Value] * input.ConstraintLimit[i]; if (Math.Abs(X[i] + frictionLimit) < 1E-50 || Math.Abs(X[i] - frictionLimit) < 1E-50) return true; else return false; case ConstraintType.JointMotor: if (Math.Abs(X[i] - input.ConstraintLimit[i]) < 1E-50 || Math.Abs(X[i] + input.ConstraintLimit[i]) < 1E-50) return true; else return false; default: return false; } }
public static void GetConstraintValues( LinearProblemProperties input, double[] x, int i, ref double Min, ref double Max) { switch (input.ConstraintType[i]) { case ConstraintType.Collision: case ConstraintType.JointLimit: Min = 0.0; Max = double.MaxValue; break; case ConstraintType.Friction: double frictionLimit = x[input.Constraints[i][0].Value] * input.ConstraintLimit[i]; Min = -frictionLimit; Max = frictionLimit; break; case ConstraintType.JointMotor: double limit = input.ConstraintLimit[i]; Min = -input.ConstraintLimit[i]; Max = input.ConstraintLimit[i]; break; default: Min = double.MinValue; Max = double.MaxValue; break; } }
public void Solve(LinearProblemProperties linearProblemProperties) { if (linearProblemProperties.Count > 0) { double internalSOR = this.solverParameters.SOR; double moduleOnlineGradient = 0.0; double modOGa = 0.0; double bk = 0.0; double test = 0.0; double[] oldX = new double[linearProblemProperties.Count]; double[] deltaf = new double[linearProblemProperties.Count]; double[] pk = new double[linearProblemProperties.Count]; double[] buffer = new double[linearProblemProperties.Count]; Array.Copy (linearProblemProperties.StartX, oldX, linearProblemProperties.Count); this.gaussSeidel ( linearProblemProperties, this.solverParameters.SOR, ref buffer); deltaf = this.calculateGradient (oldX, linearProblemProperties.StartX); this.copyNegArray (ref pk, deltaf); for (int i = 0; i < this.solverParameters.MaxIteration; i++) { Array.Copy (linearProblemProperties.StartX, oldX, linearProblemProperties.Count); moduleOnlineGradient = this.arrayModule (deltaf); if (moduleOnlineGradient < this.solverParameters.ErrorTolerance) break; if (i == 0) modOGa = moduleOnlineGradient; else { test = modOGa - moduleOnlineGradient; if (test < 0.0 && internalSOR > 0.0) internalSOR -= this.solverParameters.SORStep; modOGa = moduleOnlineGradient; } this.gaussSeidel ( linearProblemProperties, internalSOR, ref buffer); deltaf = this.calculateGradient (oldX, linearProblemProperties.StartX); bk = 0.0; if (moduleOnlineGradient != 0.0) bk = this.arrayModule (deltaf) / moduleOnlineGradient; else { bk = 0.0; continue; } if (bk <= 1.0) { for (int j = 0; j < linearProblemProperties.Count; j++) { linearProblemProperties.StartX [j] += bk * pk [j]; buffer [j] = oldX [j]; pk [j] = bk * pk [j] - deltaf [j]; this.clampX (linearProblemProperties, j); } } } } }
private double kernel( LinearProblemProperties input, SolutionValues[] X, int i) { double sumBuffer = 0.0; //Avoid last row elaboration if (i + 1 != input.Count) { double[] bufValue = input.M [i].Value; int[] bufIndex = input.M [i].Index; for (int j = 0; j < input.M [i].Count; j++) { if(bufIndex [j] > i) sumBuffer += bufValue [j] * X [bufIndex [j]].X; } } return sumBuffer; }
private SolutionValues[] calculateDirection( LinearProblemProperties input, SolutionValues[] Xk1, double[] deltaK, ref double[] searchDirection, double betak) { SolutionValues[] result = new SolutionValues[Xk1.Length]; for (int i = 0; i < Xk1.Length; i++) { double bDirection = betak * searchDirection[i]; result[i].X = Xk1[i].X + bDirection; searchDirection[i] = bDirection - deltaK[i]; result[i] = ClampSolution.Clamp(input, result, i); } return result; }
private void internalIteration( LinearProblemProperties input, ref double[] sum, ref double[] buffer, double internalSOR, int i) { int index = i * input.Count; double sumBuffer = 0.0; for (int j = 0; j < i; j++) sumBuffer += input.M [index + j] * input.X [j]; sum [i] += sumBuffer; sum [i] = (input.B [i] - sum [i]) * input.Diag [i]; input.X [i] = buffer [i] + (sum [i] - buffer [i]) * internalSOR; }
private void clampX( LinearProblemProperties input, int i) { switch (input.ConstraintType[i]) { case ConstraintType.Collision: input.X [i] = Math.Max (0.0, input.X [i]); break; case ConstraintType.StaticFriction: input.X [i] = GeometryUtilities.Clamp ( input.X [i], input.X [i + input.Constraints[i]] * input.ConstraintLimit [i], -input.X [i + input.Constraints[i]] * input.ConstraintLimit [i]); break; case ConstraintType.DynamicFriction: input.X [i] = GeometryUtilities.Clamp ( input.X [i], input.X [i + input.Constraints[i]] * input.ConstraintLimit [i], -input.X [i + input.Constraints[i]] * input.ConstraintLimit [i]); break; case ConstraintType.Joint: break; default: break; } }
private void gaussSeidel( LinearProblemProperties input, double internalSOR, ref double[] buffer) { double[] sum = new double[input.Count]; this.lowerTriangularMatrix (input, ref sum); for (int i = 0; i < input.Count; i++) { this.internalIteration (input, ref sum, ref buffer, internalSOR, i); this.clampX (input,i); buffer [i] = input.X [i]; } }
private double[] Project( LinearProblemProperties input, double[] x) { double[] result = new double[input.Count]; for (int i = 0; i < input.Count; i++) { result[i] = ClampSolution.Clamp(input, x, i); } return result; }
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; }
private void lowerTriangularMatrix( LinearProblemProperties input, ref double[] sum) { for (int i = 0; i < input.Count; i++) sum[i] = this.kernel (input, i); }
private double kernel( LinearProblemProperties input, int i) { double sumBuffer = 0.0; int index = i * input.X.Length; for (int j = 0; j < input.Count; j++) sumBuffer += input.M [index + j] * input.X [j]; return sumBuffer; }
private double[] ElaborateLowerTriangularMatrix( LinearProblemProperties input, SolutionValues[] X) { double[] sum = new double[input.Count]; Parallel.For (0, input.Count, new ParallelOptions { MaxDegreeOfParallelism = SolverParameters.MaxThreadNumber }, i => { sum [i] = kernel (input, X, i); }); return sum; }