function ddposik(o1, o2, phi1, phi2)
% circular arc trajectories for a diff drive robot from (o1, phi1) to (o2, phi2)
%
% o is the robot (x, y) position
% phi is the robot orientation in radians

ns = 128; % number of segments in circle graphics

x1 = o1(1); y1 = o1(2); dx1 = cos(phi1); dy1 = sin(phi1);
x2 = o2(1); y2 = o2(2); dx2 = cos(phi2); dy2 = sin(phi2);

clf; hold on; axis equal;

% plot initial pose in green, final in red
plot(x1, y1, 'go');
plot(x2, y2, 'rx');

quiver(x1, y1, dx1, dy1, 'g');
quiver(x2, y2, dx2, dy2, 'r');

% side-pointing vectors at start and end poses
ex1 = -dy1; ey1 = dx1; % positive left
ex2 = dy2; ey2 = -dx2; % positive right

%quiver(x1, y1, ex1, ey1, 'g:');
%quiver(x2, y2, ex2, ey2, 'r:');

% now setup and solve quadratic equation for circle radius

o1 = o1(:); o2 = o2(:); 
e1 = [ex1; ey1]; e2 = [ex2; ey2];

% c1 = o1+r*e1
% c2 = o2+r*e2
%
% ||c2-c1|| = 2*r
% ||o2+r*e2-o1-r*e1|| = 2*r
% (o2+r*e2-o1-r*e1)^2 = 4*r^2
% ((o2-o1)+r*(e2-e1))^2 = 4*r^2
% (do+r*de)^2 = 4*r^2
% do*do+2*r*de*do+r^2*de*de = 4*r^2
% r^2*(de*de-4) + r*2*de*do + do*do = 0

do = o2-o1;
de = e2-e1;

A = de'*de-4;
B = 2*de'*do;
C = do'*do;
D = B*B-4*A*C;

if (abs(A) > 0)
  ra = (-B+sqrt(D))/(2*A);
  rb = (-B-sqrt(D))/(2*A);
else
  ra = (2*C)/(-B+sqrt(D));
  rb = (2*C)/(-B-sqrt(D));
end

if (abs(ra) < abs(rb)) r = ra; else r = rb; end

% circle centers
c1 = o1+r*e1;
c2 = o2+r*e2;

% plot the circles

plot(c1(1), c1(2), 'g+');
plot(c2(1), c2(2),'r+');

plot([x1;c1(1)], [y1;c1(2)], 'g:');
plot([x2;c2(1)], [y2;c2(2)], 'r:');

ls = linspace(0, 2*pi, ns);
plot(c1(1)+r*cos(ls'), c1(2)+r*sin(ls'), 'g-');
plot(c2(1)+r*cos(ls'), c2(2)+r*sin(ls'), 'r-');

% TBD calculate turn angles

end
