function [pol] = SensorToPolar(cal,sensor)
% [pol] = SensorToPolar(cal,[sensor])
%
% Converts from sensor rectangular coordinates to polar
% coordinates.
%
% Polar coordinates are defined as radius, azimuth, and elevation.

% 9/9/93			jms		It didn't work for matrix inputs, because a 
%										matrix '^' needed to be a by-element '.^'
% 9/26/93			dhb   Added calData argument.
% 2/6/94			jms   Changed 'polar' to 'pol'
% 2/20/94			jms   Added single argument case to avoid cData
% 4/6/96			dhb		Fixed bug noted by ccc.  Need to use four quadrant
%										arctangent atan2().
% 5/20/98     dhb   Fix little bug, make sure index is not empty.
% 4/5/02      dhb, ly  New calling interface.

if (nargin==1)
  sensor=cal;
end

[n,m] = size(sensor);
if (n ~= 3)
  error('Cannot handle sensor coordinates with dimension other than 3');
end

pol = zeros(n,m);
pol(1,:) = sqrt(sensor(1,:).^2 + sensor(2,:).^2 + sensor(3,:).^2);
pol(2,:) = atan2(sensor(2,:),sensor(1,:));
index = find(pol(2,:) < 0);
if (~isempty(index))
	pol(2,index) = 2*pi*ones(size(index))+pol(2,index);
end
pol(3,:) = atan(sensor(3,:) ./ sqrt(sensor(1,:).^2 + sensor(2,:).^2) );

