function dtheta=DeltaAngle(Rx,Ry,Rt,Dx,Dy)
% Input:
%    Rx = X Start position of center of robot's body in cm
%    Ry = Y start position of center of robot's body in cm
%    Rt = Robot body initial rotation with respect to North (radians)
%    Dx = X position of the robot's destination
%    Dy = Y position of the robot's destination
% Output:
%    dtheta = Robot body's orientation with respect to the destination
%             [-pi,pi] a negative indicates that the destination is to 
%             the robot's left dtheta radians. A positive indicates
%             the destination is to the robot's right dtheta radians.
%% 
%% Simple Angle Computation
%% Copyright 2000  William Micheal Siever 

%% Determine the orientation of the Destination with respect to North:
destv=[Dx-Rx Dy-Ry];
% Normalize the destination vector:
destv=destv./sqrt(destv*destv');

% Compute the destination angle destt in [0,2*pi]
if destv(1)>0     % 1st or 2nd quadrant
    destt=  pi/2+asin(destv(2));
else              % 3rd or 4th quadrant
    destt=3*pi/2-asin(destv(2));
end

% Adjust Rt into the range [0-2*pi]
while Rt<0
    Rt=Rt+2*pi;
end
while Rt>2*pi
   Rt=Rt-2*pi;
end

% Determine the "minimum" angle 
delta1=2*pi-max(Rt,destt)+min(Rt,destt);
delta2=max(Rt,destt)-min(Rt,destt);

if delta1<delta2
    % Now determine the direction:
    if Rt==min(Rt,destt)
       dtheta=-delta1;
    else
       dtheta=delta1;
    end
else
    % Now determine the direction:
    if Rt==min(Rt,destt)
       dtheta=delta2;
    else
       dtheta=-delta2;
    end
end
