% REGROT: Given a configuration of points, of which two are landmarks,
%         registers the configuration so that the first landmark is located at 
%         (0,0), and rotates the configuration so that the second landmark is 
%         horizontal to the first.
%
%     Usage: [newpts,theta] = regrot(p1,p2,pts)
%
%         p1 =     index to registration landmark in configuration.
%         p2 =     index to rotation landmark.
%         pts =    [N x 2] matrix of coordinates of point configuration.
%         --------------------------------------------------------------
%         newpts = [N x 2] matrix of registered & rotated points.
%         theta =  angle of rotation.
%

% RE Strauss, 6/26/98
%   3/14/02 - return angle of rotation.

function [newpts,theta] = regrot(p1,p2,pts)
  [N,P] = size(pts);

  i = 1:N;
  i(p1) = [];

  pts = pts - ones(N,1)*pts(p1,:);            % Register on first landmark

  theta = angledev([-1 0],[0 0],pts,1);
  theta = theta - theta(p2);                  % Rotate to second landmark

  r = sqrt(pts(:,1).^2 + pts(:,2).^2);        % Distances from origin

  newpts = pts;
  newpts(:,1) = r.*cos(theta);                % Rectangular coordinates
  newpts(:,2) = r.*sin(theta);
  newpts(p1,:) = [0 0];
  
  theta = theta(1);

  return;

