private double[] Execute(
Func <double[], double> f,
List <Func <double[], double> > equalityConstraints,
List <Func <double[], double> > inequalityConstraints,
double[] lowerBound,
double[] upperBound,
double[] startValues,
int nIter)
{
OptVector xOld = new OptVector(startValues);
OptVector xNew = new OptVector(xOld);
OptVector lastFeasibleSolution = new OptVector(xOld);
List <Func <double[], double> > eqConstraints = new List <Func <double[], double> >();
if (equalityConstraints != null)
{
eqConstraints = new List <Func <double[], double> >(equalityConstraints);
}
List <Func <double[], double> > inqConstraints = new List <Func <double[], double> >();
if (inequalityConstraints != null)
{
inqConstraints = new List <Func <double[], double> >(inequalityConstraints);
}
var boundsConstraints = CreateBoundsConstraints(lowerBound, upperBound);
OptVector lambdaEq = new OptVector(eqConstraints.Count);
List <InequalityConstraintProperties> inequalityConstraintsProp = new List <InequalityConstraintProperties>();
foreach (var item in inqConstraints)
{
inequalityConstraintsProp.Add(new InequalityConstraintProperties(false, 0.0, item, false, 0.0, false, -1));
}
foreach (var item in boundsConstraints)
{
inequalityConstraintsProp.Add(new InequalityConstraintProperties(false, 0.0, item.Function, false, 0.0, true, item.Index));
}
OptVector[] hessian = OptVector.GetIdentity(startValues.Length);
int iqIndexStart = -1;
SetInequalityActiveConstraint(xNew, ref inequalityConstraintsProp, ref iqIndexStart);
Func <double[], double> lagrangian = BuildLagrangian(f, eqConstraints, inequalityConstraintsProp, lambdaEq.MinArray);
double directionValue = 0.0;
double lastMinValue = double.MaxValue;
double[] eqPenaltyParams = GetAndSetPenaltyParam(
f,
eqConstraints,
ref inequalityConstraintsProp,
xOld);
for (int i = 0; i < nIter; i++)
{
OptVector lambdaIq = new OptVector(inequalityConstraintsProp.Where(x => x.IsActive).Select(x => x.Lambda).ToArray());
OptVector direction = GetGradientDirection(
xNew,
eqConstraints,
lambdaEq,
inequalityConstraintsProp,
hessian,
lagrangian,
lambdaIq);
OptVector directionX = ExtractX(direction, xOld);
lambdaIq = lambdaIq + ExtractInequalityLambda(direction, xOld, lambdaEq, lambdaIq);
if (RemoveNegativeLambda(ref inequalityConstraintsProp, lambdaIq))
{
lagrangian = BuildLagrangian(f, eqConstraints, inequalityConstraintsProp, lambdaEq.MinArray);
continue;
}
directionValue = directionX * directionX;
if (directionValue < Precision)
{
inequalityConstraintsProp = SetInequalityLambda(inequalityConstraintsProp, lambdaIq);
SetInequalityActiveConstraint(xNew, ref inequalityConstraintsProp, ref iqIndexStart);
if (EarlyExit &&
!inequalityConstraintsProp.Any(x => !x.IsValid))
{
lastFeasibleSolution = xNew;
break;
}
lagrangian = BuildLagrangian(f, eqConstraints, inequalityConstraintsProp, lambdaEq.MinArray);
}
else
{
double step = ComputeStepLength(
f,
equalityConstraints,
inequalityConstraintsProp,
directionX,
xOld,
1.0,
eqPenaltyParams);
OptVector stepValue = step * directionX;
xNew = xOld + stepValue;
OptVector gradient = xNew - xOld;
if (gradient.Length() == 0.0 ||
stepValue.Length() < 1E-10)
{
OptVector nDirection = directionX.Normalize();
step = ComputeStepLength(
f,
equalityConstraints,
inequalityConstraintsProp,
nDirection,
xOld,
1.0,
eqPenaltyParams);
OptVector stVal = step * nDirection;
if (stVal.Length() < localMinCheck)
{
stepValue = nDirection;
hessian = OptVector.GetIdentity(xNew.Count);
}
else
{
stepValue = stVal;
}
xNew = xOld + stepValue;
}
if (xNew.MinArray.Contains(double.NaN))
{
xNew = lastFeasibleSolution;
xOld = xNew;
hessian = OptVector.GetIdentity(xNew.Count);
foreach (var item in inequalityConstraintsProp)
{
item.Lambda = 0.0;
}
lambdaEq = new OptVector(eqConstraints.Count);
eqPenaltyParams = GetAndSetPenaltyParam(
f,
eqConstraints,
ref inequalityConstraintsProp,
xNew);
lagrangian = BuildLagrangian(f, eqConstraints, inequalityConstraintsProp, lambdaEq.MinArray);
continue;
}
lambdaEq = lambdaEq + ExtractEqualityLambda(direction, xOld, lambdaEq);
inequalityConstraintsProp = SetInequalityLambda(inequalityConstraintsProp, lambdaIq);
SetInequalityActiveConstraint(xNew, ref inequalityConstraintsProp, ref iqIndexStart);
hessian = CalculateLagrangianHessian(lagrangian, hessian, xNew, xOld, stepValue);
lagrangian = BuildLagrangian(f, eqConstraints, inequalityConstraintsProp, lambdaEq.MinArray);
//Set penalty params
eqPenaltyParams = SetPenaltyEqParam(eqPenaltyParams, lambdaEq);
SetPenaltyIqParam(ref inequalityConstraintsProp);
//Penalty Parameters
double newEqConstraintsViolation = GetEqualityConstraintsViolation(eqConstraints, xNew);
double minValue = f(xNew.MinArray);
if (minValue < lastMinValue &&
!inequalityConstraintsProp.Any(x => !x.IsValid) &&
newEqConstraintsViolation < equalityConstraintTol)
{
lastFeasibleSolution = xNew;
lastMinValue = minValue;
}
xOld = xNew;
}
}
return(lastFeasibleSolution.MinArray);
}
private OptVector[] CalculateLagrangianHessian(
Func <double[], double> lagrangian,
OptVector[] lagrangianHessian,
OptVector xNew,
OptVector xOld,
OptVector step)
{
OptVector s = step;
OptVector y = new OptVector(numericalDerivative.EvaluatePartialDerivative(lagrangian, xNew.MinArray, 1)) -
new OptVector(numericalDerivative.EvaluatePartialDerivative(lagrangian, xOld.MinArray, 1));
OptVector[] yy = OptVector.Mult(y, y);
double ys = y * s;
OptVector partialDenom = OptVector.Mult(lagrangianHessian, s);
OptVector[] num = OptVector.Mult(partialDenom, OptVector.Mult(s, lagrangianHessian));
double denom = -1.0 * s * partialDenom;
#region Positive definiteness
if (ys < 1E-15)
{
double theta = 0.999999;
OptVector yNew = new OptVector(y);
double ysNew = ys;
while (ysNew < lambda * s * partialDenom &&
theta >= 0.0)
{
yNew = y * theta + (1.0 - theta) * partialDenom;
ysNew = yNew * s;
theta = theta - 1E-5;
}
y = yNew;
ys = ysNew;
yy = OptVector.Mult(y, y);
}
#endregion
if (ys == 0.0)
{
if (step.Length() > 0)
{
return(OptVector.GetIdentity(xNew.Count, step));
}
return(OptVector.GetIdentity(xNew.Count));
}
OptVector[] addParam1 = OptVector.Div(yy, ys);
OptVector[] addParam2 = OptVector.Div(num, denom);
return(OptVector.Sum(OptVector.Sum(lagrangianHessian, addParam1), addParam2));
}
public OptVector Solve(
OptVector[] A,
OptVector b,
OptVector startX,
int nIter)
{
OptVector[] symmA = A;
OptVector normb = b;
//Symmetrize matrix
if (CheckSymmetry)
{
OptVector[] At = OptVector.Transpose(A);
if (!OptVector.Equals(A, At))
{
symmA = OptVector.Mult(At, A);
normb = OptVector.Mult(At, b);
}
}
OptVector v0 = new OptVector(b.Count);
OptVector v1 = normb - OptVector.Mult(symmA, startX);
double beta1 = v1.Length();
double betaN = 0.0;
double n = beta1;
double c0 = 1.0;
double c1 = 1.0;
double s0 = 0.0;
double s1 = 0.0;
OptVector w0 = new OptVector(v1.Count);
OptVector w_1 = new OptVector(v1.Count);
OptVector x = new OptVector(startX);
for (int i = 0; i < nIter; i++)
{
//Calculate Lanczos Vectors
OptVector v = (1.0 / beta1) * v1;
OptVector Av = OptVector.Mult(symmA, v);
double alpha = v * Av;
v1 = Av - alpha * v - beta1 * v0;
betaN = v1.Length();
//Calculate QR factors
double lambda = c1 * alpha - c0 * s1 * beta1;
double p1 = Math.Sqrt(lambda * lambda + betaN * betaN);
double p2 = s1 * alpha + c0 * c1 * beta1;
double p3 = s0 * beta1;
//Calculate New Givens Rotations
c0 = c1;
c1 = lambda / p1;
s0 = s1;
s1 = betaN / p1;
//Update Solution
OptVector w = (1.0 / p1) * (v - p3 * w_1 - p2 * w0);
x = x + c1 * n * w;
n = -s1 * n;
residual = Math.Abs(n);
if (residual < precisionConst)
{
break;
}
beta1 = betaN;
v0 = v;
w_1 = w0;
w0 = w;
}
return(x);
}