function [dist, partials] = linkage(params, pt)
% function [dist, partials] = linkage(params, pt)
% parameters for a linkage are:
% [basposx, baseposy, theta1, len1, theta2, len2, theta3, len3, ...]
% thetas are relative to the origin, not the previous link.
% leni is the length of the ith linkage.

% This function is not optimized at all.  In fact, this is probably
% the slowest way to compute distances and partials.
%
% The correct way would be to compute edge equations, with partials,
% for each link.  The equations and partials could then be applied to
% matrices of points, rather than a point-at-a-time, as it does in
% this version.


% Search linkage for closest point

x = params(1);
y = params(2);
mindist = Inf;
mini = 0;

% check each link
for i=3:2:length(params)
  theta = params(i);
  len = params(i+1);
  basept = [x y];
  d = distlink(basept, theta, pt);
  if abs(d) < abs(mindist),
    mindist = d;
    mini = i;
    bestbase = basept;
  end
  x = x + len * cos(theta);
  y = y + len * sin(theta);
end

dist=mindist;


% compute partials relative for the edge equation of the closest link
partials = zeros(size(params));
theta = params(mini);

% partial wrt base x, y
partials(1) = - sin(theta);
partials(2) = cos(theta);

% partial wrt thetai and leni for links before the closest one
for i=3:2:mini-1,
  % angle
  partials(i) = params(i+1) * (sin(theta) * sin(params(i)) + cos(theta) * cos(params(i)));
  % len
  partials(i+1) = -sin(theta)*cos(params(i))+cos(theta)*sin(params(i));
end

% partial for the final link's theta
partials(mini) = cos(theta) * (pt(1) - bestbase(1)) + ...
    sin(theta) * (pt(2) - bestbase(2));


% edge equation.  See also Pineda's article in SIGGRAPH 1988.
function d = distlink(basept, theta, pt)
d = [pt - basept] * [sin(theta); -cos(theta)];