function [c,fail] = hermite_geometric(p,d,kappa)
% HERMITE_GEOMETRIC: planar geometric Hermite interpolant 
% c = hermite_geometric(p,d,kappa)
% positions p(1:2,:), tangent directions d(1:2,:), 
% and curvatures kappa(1:2) at endpoints, 
% determining Bezier control points c(1:4,:)
% fail = 1 indicates interpolation failure

% normalize tangent directions 
for k = 1:2
   d(k,:) = d(k,:)/norm(d(k,:));
end

% parameters for nonlinear system
dp = diff(p); 
d12 = 2*det(d);
a1 = 6*det([d(1,:);dp])/d12; a2 = 6*det([d(2,:);dp])/d12;
b1 = kappa(1)/d12; b2 = kappa(2)/d12;

% solve: s1 = -a2 - b2*s2^2, s2 = a1 - b1*s1^2
s1 = roots([b2*b1^2, 0, -2*b2*a1*b1, 1, b2*a1^2+a2]);
s2 = a1 - b1*s1.^2; 

% select admissible (approximate) solution
ind = find(s1>0 & s2>0 & imag(s1)==0 & imag(s2) == 0); 
if isempty(ind) 
   display('interpolation failed'); fail = 1;
else 
   s1 = s1(ind); s2 = s2(ind);fail = 0;
end
   
% closest solution to a linear fit: p(2,:)-p(1,:) ~ s*d
[mins,k] = min(sum(abs([s1 s2]'-norm(dp))));
s1 = real(s1(k)); s2 = real(s2(k));

% control points
c = [p(1,:); p(1,:)+s1*d(1,:)/3; p(2,:)-s2*d(2,:)/3; p(2,:)];

