function [ag,ng,tg] = linkage_ag(thdot2,thddot,r,rg,lthg,nvec,tvec)
%
% find linear acceleration of centers of mass
%
% INPUT arguments:
% thdot2(nlink) -- squares of angular velocity of links
% thddot(nlink) -- angular acceleration of links
% r(nlink)      -- length of links
% rg(nlink)     -- length from start of link to center of mass
% lthg(nlink)   -- angle in local coordinates (relative to nvec and tvec) 
%                  to the center of mass from the link start point
% nvec(2,nlink), tvec(2,nlink) -- vectors as computed by linkage_vec
%
% OUTPUT arguments:
% ag(2,nlink) -- linear acceleration of center of mass
% ng(2,nlink) -- unit vector pointing from start of link to center of mass
% tg(2,nlink) -- unit vector perpendicular to ng such that ng^tg = k, or
%                tg = k & ng
%
% We need to find ng and tg because center of mass of link is not
% necessarily along the line between the ends of the link. The vectors 
% ng and tg are used to set the location of the center of mass, and 
% they are calculated by rotating nvec and tvec counterclockwise by 
% the angle lthg 
%
% More specifically, the variable lthg gives the angle between the line of
% the link and the line from the starting point of the link to the center
% of mass; ng is a unit vector pointing from the start of the link to the 
% center of mass and tg is contructed such that ng ^ tg = k, or
% tg = k ^ ng.
%
nlink = length(thddot); 
%
ag = zeros(2,nlink);
ng = zeros(2,nlink);
tg = zeros(2,nlink);
%
% Move along the links, adding up the acceleration as we go;
% a_base is the linear accleration of the beginning of the current link
% and is set to zero at the start because the first link is pinned to 
% "ground". For each link, we find the acceleration of the center of 
% gravity and also update a_base to be the acceleration of the end of the 
% current link (and thus of the beginning of the next link).
% 
a_base = [0.0 0.0]'; % pin beginning of first link to "ground"
%
% loop over the links
for i_link = 1:nlink;
   %
   % rotate nvec and tvec by lthg to get ng and tg
   %
   ng(:,i_link) = ...
      nvec(:,i_link)*cos(lthg(i_link)) + tvec(:,i_link)*sin(lthg(i_link));
   tg(:,i_link) = ...
     -nvec(:,i_link)*sin(lthg(i_link)) + tvec(:,i_link)*cos(lthg(i_link));
   %
   % calculate the acceleration of the center of mass
   %
   ag(:,i_link) = a_base + ...
      -thdot2(i_link)*rg(i_link)*ng(:,i_link) + ...
      +thddot(i_link)*rg(i_link)*tg(:,i_link);
   %
   % update a_base to be the acceleration of the end of the link, and thus
   % the acceleration of the start of the next link
   %
   a_base = a_base + ...
      -thdot2(i_link)*r(i_link)*nvec(:,i_link) + ...
      +thddot(i_link)*r(i_link)*tvec(:,i_link);
end;
