function [Elongation,ElongationRate,s] = TetherElongations(xo1,pos1,vel1,p_a1,xo2,pos2,vel2,p_a2)

%Purpose:  Compute Elongation, Elongation Rate, and direction from attachment point 
%          on body 1 to the attachment point on body 2
% Input: x01 => nominal origin of body 1 in global coordinate system
%        pos1 => pos1(1:6) = 6 dof positions of body 1
%        vel1 => vel1(1:6) = 6 dof velocities of body 1
%        x02 => nominal origin of body 2 in global coordinate system
%        pos2 => pos2(1:6) = 6 dof positions of body 2
%        vel2 => vel2(1:6) = 6 dof velocities of body 2
% Output:  E = Elongation of tether relative to nominal positions
%          Edot = rate of Elongation change
%          s = vector that represents direction from attachment point on
%          body 1 to attachment point on body 2.


%Make sure x01, x02, p_a1 and p_a2 are column vectors
p_size = size(p_a1);
if(p_size(2) > p_size(1)) 
    p_a1 = p_a1'; 
end
p_size = size(p_a2);
if(p_size(2) > p_size(1))
    p_a2 = p_a2';
end
p_size = size(xo1);
if(p_size(2) > p_size(1)) 
    xo1 = xo1'; 
end
p_size = size(xo2);
if(p_size(2) > p_size(1))
    xo2 = xo2';
end
clear p_size;


%Point A is on body 1 and Point b is on body 2
%Point A is the attachment point on body 1 in the GCS in the nominal position
%Point Ap is the attachment point on body 1 in the GCS after translation and
%    rotation specified by the state vector
%Point B is the attachment point on body 2 in the GCS in the nominal position
%Point Bp is the attachment point on body 2 in the GCS after translation and
%    rotation specified by the state vector

alpha = pos1(5);
theta = pos2(5);

%Initial position relative to GCS
pointA = xo1+p_a1;
pointB = xo2+p_a2;
AB = pointB - pointA;
length_AB = norm(AB,2);

%Position of attachment point after rotation
R1 = eye(3);
R1(1,1) = cos(alpha); R1(1,3) = sin(alpha);
R1(3,1) = -R1(1,3);  R1(3,3) = R1(1,1);
R2 = eye(3);
R2(1,1) = cos(theta); R2(1,3) = sin(theta);
R2(3,1) = -R2(1,3);  R2(3,3) = R2(1,1);


pointAp = xo1+R1*p_a1+pos1(1:3);
pointBp = xo2+R2*p_a2+pos2(1:3);

ApBp = pointBp - pointAp;
length_ApBp = norm(ApBp,2);

Elongation = length_ApBp-length_AB;

%Velocity of attachment point in GCS
R1v = zeros(3);
R1v(1,1) = -vel1(5)*sin(alpha); R1v(1,3) = vel1(5)*cos(alpha);
R1v(3,1) = -R1v(1,3);  R1v(3,3) = R1v(1,1);
R2v = zeros(3);
R2v(1,1) = -vel2(5)*sin(theta); R2v(1,3) = vel2(5)*cos(theta);
R2v(3,1) = -R2v(1,3);  R2v(3,3) = R2v(1,1);

Va = vel1(1:3)+R1v*p_a1;
Vb = vel2(1:3)+R2v*p_a2;

ElongationRate = dot(ApBp/norm(ApBp,2),Vb-Va);

s = ApBp/norm(ApBp,2);

