function [ Q, P ] = shape(X, ref, f, g);
%
% SHAPE: Q = shape(X, ref, f, g);
%
% Map features f and g to fixed reference positions.
%
% INPUT:
% ----- 
% X   - (2p X N)  Each column is: [x1, x2, ..., xp, y1, y2, ..., yp]'
% ref - (2p X 1)  reference positions
% f,g - (scalars) indices of the two features to be mapped to 
%                   reference positions
%
% OUTPUT:
% ------
% Q   - (2p X N) shape variables
% P   - (2 x N)  pose variables
%
% NOTE: 
% ----
% Q(f) = ref(f); Q(p+f) = ref(p+f);mt = 
% Q(g) = ref(g); Q(p+g) = ref(p+g);
%

p = size(X,1)/2;
N = size(X,2);
J = sqrt(-1);

r = ref(1:p) + J * ref(p+1:2*p); % Convert to complex form
z = X(1:p,:) + J * X(p+1:2*p,:);

Q = zeros(2*p,N);
P = zeros(2, N);

for j = 1 : N

  	% Determine transformation
  	m = (r(g) - r(f)) / (z(g,j) - z(f,j));
  	b = r(f) - m * z(f,j);

  	% Apply transformation
  	w = m * z(:,j) + b;

  	% Put results back into a real vector
  	Q(1:p, j) = real(w);
  	Q(p+1:2*p, j) = imag(w);

	% Compute scale and orientation

	P(1, j) = 1 / abs(m);
	P(2, j) = - angle(m);

end