double[] objectiveGradient(double[] x)
{
// Compute
//
// Phi'(x) = f'(x) + rho sum(c_i(x)*c_i'(x)) - sum(lambda_i * c_i'(x))
//
double[] orig = Gradient(x);
double[] g = new double[NumberOfVariables];
for (int i = 0; i < g.Length; i++)
g[i] = orig[i];
double[] sum = new double[x.Length];
double[] weightedSum = new double[x.Length];
// For each equality constraint
for (int i = 0; i < equalityConstraints.Length; i++)
{
double c = equalityConstraints[i].Function(x) - equalityConstraints[i].Value;
double[] cg = equalityConstraints[i].Gradient(x);
for (int j = 0; j < cg.Length; j++)
{
sum[j] += rho * c * cg[j];
weightedSum[j] += lambda[i] * cg[j];
}
}
// For each "lesser than" inequality constraint
for (int i = 0; i < lesserThanConstraints.Length; i++)
{
double c = lesserThanConstraints[i].Function(x) - lesserThanConstraints[i].Value;
double[] cg = lesserThanConstraints[i].Gradient(x);
if (c > 0)
{
// Constraint is being violated
for (int j = 0; j < cg.Length; j++)
{
sum[j] += rho * c * cg[j];
weightedSum[j] += mu[i] * cg[j];
}
}
}
// For each "greater-than" inequality constraint
for (int i = 0; i < greaterThanConstraints.Length; i++)
{
double c = -greaterThanConstraints[i].Function(x) + greaterThanConstraints[i].Value;
double[] cg = greaterThanConstraints[i].Gradient(x);
if (c > 0)
{
// Constraint is being violated
for (int j = 0; j < cg.Length; j++)
{
sum[j] += rho * c * -cg[j];
weightedSum[j] += nu[i] * -cg[j];
}
}
}
for (int i = 0; i < g.Length; i++)
g[i] += sum[i] - weightedSum[i];
return g;
}