function [Qnet] = ArcXY(P1, P2, P3, T)
% Draw an arc in the XY plane from P1 to P3 passing through P2
% in T seconds
% rev 5/26/15

t = [0:0.01:T];
a = t/T;
a = (1 - cos(pi*a))/2;

X1 = P1(1);
Y1 = P1(2);

X2 = P2(1);
Y2 = P2(2);

X3 = P3(1);
Y3 = P3(2);

A = 2*[ X2-X1, Y2-Y1 ; X3 - X1 , Y3 - Y1 ];
B = [X2^2 - X1^2 + Y2^2 - Y1^2 ; X3^2 - X1^2 + Y3^2 - Y1^2];

 P0 = inv(A)*B;
 
 X0 = P0(1);
 Y0 = P0(2);
 
 r = sqrt( (X1 - X0)^2 + (Y1-Y0)^2 );
 
 Q0 = atan2(Y1 - Y0,  X1 - X0);
 Q3 = atan2(Y3 - Y0,  X3 - X0);
 
 if (Q0 < 0) 
     Q0 = Q0 + 2*pi;
 end
 if (Q3 < 0)
     Q3 = Q3 + 2*pi;
 end
 

 
  if ((Q3 - Q0) > pi)
      Q3 = Q3 - 2*pi;
      end
  if ((Q3 - Q0) < -pi)
      Q3 = Q3 + 2*pi;
      end
  
 Q = (1-a)*Q0 + a*Q3;
 

 
 X = X0 + r*cos(Q);
 Y = Y0 + r*sin(Q);
 Z = P1(3) + 0*Q;

TIP = [X ; Y ; Z];

npt = length(TIP);

Qnet = [];

for i=1:npt
   Q = InversePuma(TIP(:,i));
   Qnet = [Qnet, Q];
   Puma(Q, TIP);
   end

P1 = TIP(:,npt);

end