function F = SD_tether(body1,bodies,n,name,p_a1,p_a2,k,b)

j = 0;
for i = 1:length(bodies)
name2 = get(bodies{i},'name');
  if(strcmp(name2,name))
    j = i;
  end
end
if(j == 0)
   error('ERROR: Can not find name match in SD_tether');
end

body2 = bodies{j};

sv1 = get(body1,'sv');
sv2 = get(body2,'sv');

%Compute force and moment due to spring/damper between attachement point
% a1 on body1 and a2 on body 2;

F = zeros(1,6);

xa = p_a1(1);      
za = p_a1(3);

xb = p_a2(1);
zb = p_a2(3);

xoa = get(body1,'x0');
xob = get(body2,'x0');

x1 = sv1(1);
z1 = sv1(3);

x2 = sv2(1);
z2 = sv2(3);

alpha = sv1(5);
theta = sv2(5);

%Initial position relative to GCS

pointA = [xoa(1) + xa     xoa(3) + za]; 
pointB = [xob(1) + xb     xob(3) + zb];

AB = pointB -pointA;

%Position After rotation and translation relative to GCS
vecr = [xa*cos(alpha) + za*sin(alpha)     za*cos(alpha) - xa*sin(alpha)];

pointAp = [xoa(1) + x1 + xa*cos(alpha) + za*sin(alpha)    xoa(3) + z1 + za*cos(alpha) - xa*sin(alpha)]; 
pointBp = [xob(1) + x2 + xb*cos(theta) + zb*sin(theta)    xob(3) + z2 + zb*cos(theta) - xb*sin(theta)]; 

ApBp = pointBp - pointAp;


F(1) = k*(ApBp(1) - AB(1));
F(3) = k*(ApBp(2) - AB(2));

%F(5) = -vecr(1)*F(3)+ vecr(2)*F(1);
%damping effect not included

dx1dt = sv1(7);
dx2dt = sv2(7);
dalphadt = sv1(11);

dz1dt = sv1(9);
dz2dt = sv2(9);
dthetadt = sv2(11);

dLdtx = dx2dt - xb*sin(theta)*dthetadt + zb*cos(theta)*dthetadt -(dx1dt - xa*sin(alpha)*dalphadt + za*cos(alpha)*dalphadt);

dLdtz = dz2dt - zb*sin(theta)*dthetadt - xb*cos(theta)*dthetadt -(dz1dt - za*sin(alpha)*dalphadt - xa*cos(alpha)*dalphadt);

F(1) = F(1) + b*dLdtx;

F(3) = F(3) + b*dLdtz;

F(5) = -vecr(1)*F(3)+ vecr(2)*F(1);

end






