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