function [xStar,yStar,dist] = E2Pdist(A,B,C,D,E,F,x0,y0)
% Ax^2 + Bxy + Cy^2 + Dx + Ey + F = 0 defines an ellipse in the plane.
% (xStar,yStar) is the closest point on the ellipse to (x0,y0) and dist is the
% euclidean distance between them.

% Get parametric representation...

M0 = [F D/2 E/2; D/2 A B/2; E/2 B/2 C];
M = [A B/2; B/2 C];
lambda = sort(diag(schur(M)));
r = det(M0)/det(M);
a = sqrt(-r/lambda(1));               % Major semi-axis
b = sqrt(-r/lambda(2));               % Minor semi-axis
if abs(lambda(1)-A) > abs(lambda(1)-C)
    t = a; a = b; b = t;
end
h = (B*E - 2*C*D)/(4*A*C - B^2);      % x-coord of center
k = (B*D - 2*A*E)/(4*A*C - B^2);      % y-coord of center  
tau = acot((A-C)/B)/2;                % Rotation angle

% The equivalent "Canonical problem" is obtained by moving the ellipse so that
% it is centered at (0,0) and its axes are aligned with the coordinate
% axes. Of course we must move (x0,y0) the same way...

Q = [cos(tau) -sin(tau); sin(tau) cos(tau)];  
v = Q'*[x0-h;y0-k];

% Now find the nearest point on (x/a)^2 + (y/b)^2 = 1 to (v(1),v(2)).
% This ellipse is parameterized by (x(t),y(t)) = (a*cos(t),b*sin(t))
% where 0<=t<=2*pi. To that end, we want to use fminbnd to minimize
%
%       rho(t) = (a*cos(t) - v(1))^t + (b*sin(t) - v(2))^2
%
% If tStar does the job then (xS,yS) = (a*cos(tStar),b*sin(tStar)) is
% the nearest point on the canonical ellipse to (v(1),v(2)). Observing
% that (xS,yS) will be in the same quadrant as (v(1),v(2)) we are able
% to hand over to fminbnd a search interval that is better than [0,2pi]:

if 0<=v(1) && 0<= v(2)
    % First quadrant...
    L = 0; R = pi/2;
elseif v(1)<=0 && 0<=v(2)
    % Second quadrant...
    L = pi/2; R = pi;
elseif v(1)<=0 && v(2)<=0
    % Third quadrant...
    L = pi; R = 3*pi/2;
else
    % Fourth quadrant...
    L = 3*pi/2; R = 2*pi;
end
tStar = fminbnd(@(t) rho(t,v,a,b),L,R);
    
xS = a*cos(tStar); yS = b*sin(tStar); dist = rho(tStar,v,a,b);
% Back to original ellipse via "unrotation and untranslation:...
z = Q*[xS;yS]; xStar = z(1) + h; yStar = z(2) + k;

function d = rho(t,v,a,b)
% Distance function...
d = sqrt((v(1) - a*cos(t))^2 + (v(2) - b*sin(t))^2);
