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;