/// <summary>Solves a quadratic programming problem 1/2 x'Px + q'x subject to Mx less or equal d and Ax = b</summary> /// <param name="x0">Start point x0</param> /// <param name="lambda0">Start dual point lambda0</param> /// <param name="nu0">Start nus (equality constraints)</param> /// <param name="P">Positive semidefinite quadratic objective matrix P</param> /// <param name="q">Linear objective q</param> /// <param name="M">Ineq constraint matrix M</param> /// <param name="d">Ineq constraint right hand side d</param> /// <param name="A">Constraint matrix A</param> /// <param name="b">Constraint right hand side b</param> /// <param name="sf">Stop function. The system won't check feasibility if this function is not null. Return true when you want the optimization to stop based on x, lambda and nu</param> public float[] SolvePrimalDual(float[] x0, float[] lambda0, float[] nu0, floatLinalg.floatSymPosDefMatrix P, float[] q, float[,] M, float[] d, float[,] A, float[] b, StopFunc sf) { //Number of primal vars int n = x0.Length; //Number of dual vars int m = lambda0.Length; //Constraint variables int c = nu0 == null ? 0 : nu0.Length; if (P!=null && P.getN != n) throw new Exception("Incompatible matrix P dimensions"); if (M.GetLength(0) != m || M.GetLength(1) != n) throw new Exception("Incompatible matrix M dimensions"); if (q.Length != n) throw new Exception("Incompatible vector q dimensions"); if (d.Length != m) throw new Exception("Incompatible vector d dimensions"); if (nu0 == null && A != null) throw new Exception("Equality constraint matrix was given. Please also give initial values for laplace multipliers nu0"); CLP = P; if (CLP != null && !CLP.IsMatrixInClMemoryUpdated) { CLP.CLValues.WriteToDevice(CLP.Values); CLP.IsMatrixInClMemoryUpdated = true; } if (c > 0) { if (b.Length != c) throw new Exception("Incompatible vector b dimensions"); if (A.GetLength(0) != c || A.GetLength(1) != n) throw new Exception("Incompatible matrix A dimensions"); } CLM = new floatLinalg.floatMatrix(M); CLx = new floatLinalg.floatVector(x0); CLxPlus = new floatLinalg.floatVector(x0); CLlambda = new floatLinalg.floatVector(lambda0); CLlambdaPlus = new floatLinalg.floatVector(lambda0); CLDeltaX = new floatLinalg.floatVector(x0); CLDeltaLambda = new floatLinalg.floatVector(lambda0); CLq = new floatLinalg.floatVector(q); CLd = new floatLinalg.floatVector(d); //Auxiliary vars zerosN = new floatLinalg.floatVector(new float[n]); float[] fOnes = new float[m]; for (int i = 0; i < fOnes.Length; i++) fOnes[i] = 1; onesM = new floatLinalg.floatVector(fOnes); Hpd = new floatLinalg.floatSymPosDefMatrix(n); MtDzM = new floatLinalg.floatSymPosDefMatrix(n); temp = new floatLinalg.floatVector(new float[n]); temp2 = new floatLinalg.floatVector(new float[n]); rhsXpd = new floatLinalg.floatVector(new float[n]); Mxd = new floatLinalg.floatVector(new float[m]); z2 = new floatLinalg.floatVector(new float[m]); z = new floatLinalg.floatVector(new float[m]); rCent = new floatLinalg.floatVector(new float[m]); tempM = new floatLinalg.floatVector(new float[m]); tempM2 = new floatLinalg.floatVector(new float[m]); CLPx = new floatLinalg.floatVector(new float[n]); //Equality constraint variables if (c > 0) { CLA = new floatLinalg.floatMatrix(A); CLb = new floatLinalg.floatVector(b); CLnu = new floatLinalg.floatVector(nu0); CLDeltaNu = new floatLinalg.floatVector(nu0); rPri = new floatLinalg.floatVector(new float[c]); Atnu = new floatLinalg.floatVector(new float[n]); tempC = new floatLinalg.floatVector(new float[c]); CLnuPlus = new floatLinalg.floatVector(new float[c]); AinvHAt = new floatLinalg.floatSymPosDefMatrix(c); AinvHrhs = new floatLinalg.floatVector(new float[c]); tempAinvMatrix = new floatLinalg.floatMatrix(new float[n, c]); fOnes = new float[c]; for (int i = 0; i < fOnes.Length; i++) fOnes[i] = 1; onesC = new floatLinalg.floatVector(fOnes); } else { CLA = null; CLb = null; CLnu = null; CLnuPlus = null; } //Checks feasibility bool feasible = true; float[] xfeas; if (sf == null) { xfeas = CheckFeasibility(x0, M, d, A, b); feasible = (xfeas != null); if (CLCalc.CLAcceleration == CLCalc.CLAccelerationType.UsingCL) CLx.CLValues.WriteToDevice(xfeas); else for (int i = 0; i < xfeas.Length; i++) CLx.Values[i] = xfeas[i]; } if (SolutionLog.KeepLog) { CLx.ReadFromDevice(); SolutionLog.PtSequence.Add((float[])CLx.Values.Clone()); } if (feasible) { float surrogDualityGap = 100; float t = 0.1f; int MAXITER = 60; int curIter = 0; while (surrogDualityGap > 5e-5f && curIter < MAXITER && (sf == null || (sf != null && !sf(CLx, CLlambda, CLnu))) ) { if (curIter == (MAXITER >> 1)) { //DEBUG, taking too long to converge for (int i = 0; i < CLlambda.Values.Length; i++) CLlambda.Values[i] = 0.1f; if (CLCalc.CLAcceleration == CLCalc.CLAccelerationType.UsingCL) CLlambda.CLValues.WriteToDevice(CLlambda.Values); t *= 0.1f; } //PDResidual(CLx, CLlambda, CLnu, t); ComputePrimalDualSearchDir(t); PrimalDualLineSearch(CLx, Mxd, CLlambda, CLnu, CLDeltaX, CLDeltaLambda, CLDeltaNu, CLxPlus, CLlambdaPlus, CLnuPlus, CompRestric, PDResidual, ref t, out surrogDualityGap); curIter++; if (SolutionLog.KeepLog) { SolutionLog.SurrDualityGaps.Add(surrogDualityGap); CLx.ReadFromDevice(); SolutionLog.PtSequence.Add((float[])CLx.Values.Clone()); } } SolutionLog.Iterations = curIter; //ReadAll(); //Copies lambdas and nus CLlambda.ReadFromDevice(); for (int i = 0; i < m; i++) lambda0[i] = CLlambda.Values[i]; if (CLnu != null) { CLnu.ReadFromDevice(); for (int i = 0; i < c; i++) nu0[i] = CLnu.Values[i]; } CLx.ReadFromDevice(); return CLx.Values; } else return null; //Not feasible }
/// <summary>Checks if it's possible to satisfy Mx less than d and Ax = b. Returns a feasible point if so</summary> /// <param name="x0">Initial guess</param> /// <param name="M">Inequality constraint matrix</param> /// <param name="d">Inequality rhs</param> /// <param name="A">Equality constr matrix</param> /// <param name="b">Equality rhs</param> public static float[] CheckFeasibility(float[] x0, float[,] M, float[] d, float[,] A, float[] b) { //Computes Mx-d floatLinalg.floatMatrix CLM = new floatLinalg.floatMatrix(M); floatLinalg.floatVector CLd = new floatLinalg.floatVector(d); floatLinalg.floatVector CLx = new floatLinalg.floatVector(x0); floatLinalg.floatMatrix CLA = null; floatLinalg.floatVector CLb = null; if (A != null) { CLA = new floatLinalg.floatMatrix(M); CLb = new floatLinalg.floatVector(d); } floatLinalg.floatVector Mxd = new floatLinalg.floatVector(new float[M.GetLength(0)]); if (CheckFeasibility(CLx, CLM, CLd, CLA, CLb, Mxd)) { CLx.ReadFromDevice(); return (float[])CLx.Values.Clone(); } else return null; }