function [dist, gradient] = Rdist_grad(pts)
% function [D, G] = Rdist_grad(pts)
%   computes the distance field D and gradient G at each point in
%   PTS, an Nx2 array.  This function is pretty slow, mostly
%   because of the distance-to-curve computation, which involves a
%   loop.

x = pts(:, 1);
y = pts(:, 2);

% The R is two rectangles and a 3 segment bezier curve
[r1, gr1] = rect_dist(x, y, 0.2, 0.1, 0.3, 0.90);
[r2, gr2] = rect_dist(x, y, 0.73, 0.091, 0.76, 0.11);
[c1, gc1] = bezier_dist(x, y, 0.37, 0.36, 0.30, 0.3832, 0.32, 0.46, .40, ...
    0.43, 100, 0.01); 
[c2, gc2] = bezier_dist(x, y, 0.40, 0.43, 0.64, 0.34, 0.5, 0.1, 0.75, ...
    0.1, 100, 0.01); 
[c3, gc3] = bezier_dist(x, y, 0.3, 0.89, 0.9, 0.90, 0.75, 0.25, 0.37, ...
    0.36, 180, 0.01); 

[dist, i] = max([r1, r2, c1, c2, c3], [], 2);
grx = [gr1(:, 1) gr2(:, 1) gc1(:, 1) gc2(:, 1) gc3(:, 1)];
gry = [gr1(:, 2) gr2(:, 2) gc1(:, 2) gc2(:, 2) gc3(:, 2)];
s = size(grx);
gradient = [grx(sub2ind(s, (1:s(1))', i)) gry(sub2ind(s, ...
      (1:s(1))', i))];
gradient(isnan(gradient)) = 0;




function [rd, grad] = rect_dist(x, y, x1, y1, x2, y2)
% distance from rectangle function
x1 = repmat(x1, size(x));
y1 = repmat(y1, size(x));
x2 = repmat(x2, size(x));
y2 = repmat(y2, size(x));
pts = [x y];
rd = sum((pts - [x1 y1]).^2, 2);
grad = [x1 y1] - pts;
[rd, grad] = find_closer(rd, grad, [x2 y1] - pts, [x2 y1] - pts);
[rd, grad] = find_closer(rd, grad, [x1 y2] - pts, [x1 y2] - pts);
[rd, grad] = find_closer(rd, grad, [x2 y2] - pts, [x2 y2] - pts);
in = find((x > x1) & (x < x2));
if (~ isempty(in)),
  
  [rd(in), grad(in, :)] = find_closer(rd(in), grad(in, :), [x(in) y1(in)] - pts(in, :), repmat([0 1], size(in), 1));
  [rd(in), grad(in, :)] = find_closer(rd(in), grad(in, :), [x(in) y2(in)] - pts(in, :), repmat([0 -1], size(in), 1));
end
in = find((y > y1) & (y < y2));
if (~ isempty(in)),
  [rd(in), grad(in, :)] = find_closer(rd(in), grad(in, :), [x1(in) y(in)] - pts(in, :), repmat([1 0], size(in), 1));
  [rd(in), grad(in, :)] = find_closer(rd(in), grad(in, :), [x2(in) y(in)] - pts(in, :), repmat([-1 0], size(in), 1));
end
rd = sqrt(rd);
neginds = ((x < x1) | (x > x2) | (y < y1) | (y > y2));
rd(neginds) = - rd(neginds);
wstate = warning('off');
grad = grad ./ repmat(sqrt(sum(grad.^2, 2)), 1, 2);
warning(wstate)

function [d, grad] = find_closer(d, grad, delta, newgrad)
newd = sum(delta.^2, 2);
inds = (newd < d);
d(inds) = newd(inds);
grad(inds, :) = newgrad(inds, :);



function [bd, grad] = bezier_dist(x, y, x1, y1, x2, y2, x3, y3, x4, y4, ...
    steps, radius)
% distance from bezier curve function
dx1 = x1 - x;
dx2 = x2 - x;
dx3 = x3 - x;
dx4 = x4 - x;
dy1 = y1 - y;
dy2 = y2 - y;
dy3 = y3 - y;
dy4 = y4 - y;

bd = sqdist(dx1, dy1);
cp = repmat([x1 y1], size(bd, 1), 1);
for i = 1:steps,
  t = i / steps;
  dists = sqdist(bez4(dx1, dx2, dx3, dx4, t), bez4(dy1, dy2, ...
      dy3, dy4, t));
  inds = find(dists < bd);
  if (~ isempty(inds)),
    bd(inds) = dists(inds);
    cp(inds, 1) = bez4(x1, x2, x3, x4, t);
    cp(inds, 2) = bez4(y1, y2, y3, y4, t);
  end
end
bd = radius - sqrt(bd);
grad = cp - [x y];
wstate = warning('off');
grad = grad ./ repmat(sqrt(sum(grad.^2, 2)), 1, 2);
warning(wstate);


function sd = sqdist(x, y)
sd = x.*x + y.*y;


function b4 = bez4(a, b, c, d, t)
a = linear(a,b,t);
b = linear(b,c,t);
c = linear(c,d,t);
a = linear(a,b,t);
b = linear(b,c,t);
b4 = linear(a,b,t);


function l = linear(a,b,t)
l = a + (b - a) * t;