%% Obtain Best Feasible Point
% This example shows how to obtain the best feasible point encountered by fmincon.
% The helper function bigtoleft is a cubic polynomial objective function in a three-dimensional variable x that grows rapidly negative as the x(1) coordinate becomes negative. Its gradient is a three-element vector. The code for the bigtoleft helper function appears at the end of this example.
% The constraint set for this example is the intersection of the interiors of two cones—one pointing up, and one pointing down. The constraint function is a two-component vector containing one component for each cone. Because this example is three-dimensional, the gradient of the constraint is a 3-by-2 matrix. The code for the twocone helper function appears at the end of this example.
% Create a figure of the constraints colored using the objective function by calling the plottwoconecons function, whose code appears at the end of this example.

figure1 = plottwoconecons;

%
%% Create Options To Use Derivatives
% To enable fmincon to use the objective gradient and constraint gradients, set appropriate options. Choose the 'sqp' algorithm.

options = optimoptions('fmincon','Algorithm','sqp',...
    "SpecifyConstraintGradient",true,"SpecifyObjectiveGradient",true);

% To examine the solution process, set options to return iterative display.

options.Display = 'iter';

%
%% Minimize Using Derivative Information
% Set the initial point x0 = [-1/20,-1/20,-1/20].

x0 = -1/20*ones(1,3);

% The problem has no linear constraints or bounds. Set those arguments to [].

A = [];
b = [];
Aeq = [];
beq = [];
lb = [];
ub = [];

% Solve the problem.

[x,fval,eflag,output] = fmincon(@bigtoleft,x0,...
           A,b,Aeq,beq,lb,ub,@twocone,options);

%
%% Examine Solution and Solution Process
% Examine the solution, objective function value, exit flag, and number of function evaluations and iterations.

disp(x)
disp(fval)
disp(eflag)
disp(output.constrviolation)

% The objective function value is smaller than –1e250, a very negative value. The constraint violation is larger than 1e85, a large amount that is still much smaller in magnitude than the objective function value. The exit flag also shows that the returned solution is infeasible.
% To recover the best feasible point that fmincon encounters, along with its objective function value, display the output.bestfeasible structure.

disp(output.bestfeasible)

% The bestfeasible point is not a good solution, as you see in the next section. It is simply the best feasible point that fmincon encountered in its iterations. In this case, even though bestfeasible is not a solution, it is a better point than the returned infeasible solution.

table([fval;output.bestfeasible.fval],...
    [output.constrviolation;output.bestfeasible.constrviolation],...
    'VariableNames',["Fval" "Constraint Violation"],'RowNames',["Final Point" "Best Feasible"])

%
%% Improve Solution: Set Bounds
% There are several ways to obtain a feasible solution. One way is to set bounds on the variables.

lb = -10*ones(3,1);
ub = -lb;
[xb,fvalb,eflagb,outputb] = fmincon(@bigtoleft,x0,...
           A,b,Aeq,beq,lb,ub,@twocone,options);

% The iterative display shows that fmincon starts at a feasible point (feasibility 0), spends a few iterations infeasible, again reaches 0 feasibility, then has small but nonzero infeasibility for the remaining iterations. The solver again reports that it found a lower feasible value at a point other than the final point xb. View the final point and objective function value, and the reported feasible point with lower objective function value.

disp(xb)
disp(fvalb)
disp(outputb.bestfeasible)

% The constraint violation at the bestfeasible point is about 4.113e-7. In the iterative display, this infeasibiliity occurs at iteration 11. The reported objective function value at that iteration is -2.894126e3, which is slightly less than the final value of -2.894125e3. The final point has lower infeasibility and first-order optimality measure. Which point is better? They are nearly the same, but each point has some claim to being better.  To see the solution details, set the display format to long.

format long
table([fvalb;outputb.bestfeasible.fval],...
    [outputb.constrviolation;outputb.bestfeasible.constrviolation],...
    [outputb.firstorderopt;outputb.bestfeasible.firstorderopt],...
    'VariableNames',["Function Value" "Constraint Violation" "First-Order Optimality"],...
    'RowNames',["Final Point" "Best Feasible"])

%
%% Improve Solution: Use Another Algorithm
% Another way to obtain a feasible solution is to use the 'interior-point' algorithm, even without bounds,

lb = [];
ub = [];
options.Algorithm = "interior-point";
[xip,fvalip,eflagip,outputip] = fmincon(@bigtoleft,x0,...
           A,b,Aeq,beq,lb,ub,@twocone,options);

% The iterative display again shows fmincon reaching points that are infeasible in its search for a solution, and fmincon again issues a message that it encountered a feasible point with lower objective function value.

       disp(xip)
disp(fvalip)
disp(outputip.bestfeasible)

% Again, the two solutions are nearly the same, and the bestfeasible solution comes from the iteration before the end. The final solution xip has better first-order optimality measure and feasibility, but the bestfeasible solution has slightly lower objective function value and an infeasibility that it not too large.

table([fvalip;outputip.bestfeasible.fval],...
    [outputip.constrviolation;outputip.bestfeasible.constrviolation],...
    [outputip.firstorderopt;outputip.bestfeasible.firstorderopt],...
    'VariableNames',["Function Value" "Constraint Violation" "First-Order Optimality"],...
    'RowNames',["Final Point" "Best Feasible"])

% Finally, reset the format to the default short.

format short

%
%% Helper Functions
% This code creates the bigtoleft helper function.

function [f gradf] = bigtoleft(x)
% This is a simple function that grows rapidly negative
% as x(1) becomes negative
%
f = 10*x(:,1).^3 + x(:,1).*x(:,2).^2 + x(:,3).*(x(:,1).^2 + x(:,2).^2);

if nargout > 1

   gradf=[30*x(1)^2+x(2)^2+2*x(3)*x(1);
       2*x(1)*x(2)+2*x(3)*x(2);
       (x(1)^2+x(2)^2)];

end
end

% This code creates the twocone helper function.

function [c ceq gradc gradceq] = twocone(x)
% This constraint is two cones, z > -10 + r
% and z < 3 - r

ceq = [];
r = sqrt(x(1)^2 + x(2)^2);
c = [-10+r-x(3);
    x(3)-3+r];

if nargout > 2

    gradceq = [];
    gradc = [x(1)/r,x(1)/r;
       x(2)/r,x(2)/r;
       -1,1];

end
end

% This code creates the function that plots the nonlinear constraints.

function figure1 = plottwoconecons
% Create figure
figure1 = figure;

% Create axes
axes1 = axes('Parent',figure1);
view([-63.5 18]);
grid('on');
hold('all');

% Set up polar coordinates and two cones
r = linspace(0,6.5,14);
th = 2*pi*linspace(0,1,40);
x = r'*cos(th);
y = r'*sin(th);
z = -10+sqrt(x.^2+y.^2);
zz = 3-sqrt(x.^2+y.^2);

% Evaluate objective function on cone surfaces
newxf = reshape(bigtoleft([x(:),y(:),z(:)]),14,40)/3000;
newxg = reshape(bigtoleft([x(:),y(:),z(:)]),14,40)/3000;

% Create lower surf with color set by objective
surf(x,y,z,newxf,'Parent',axes1,'EdgeAlpha',0.25);

% Create upper surf with color set by objective
surf(x,y,zz,newxg,'Parent',axes1,'EdgeAlpha',0.25);
axis equal
xlabel 'x(1)'
ylabel 'x(2)'
zlabel 'x(3)'
end

% Copyright 2020 The MathWorks, Inc.
%