function [ P ] = shapeDensityDM ( X, nu, Omega, refFeat )

%
%  [ P ] = shapeDensityDM ( X, nu, Omega )
%
% Convert the configuration of points in X to shape variables and
% evaluate the probability density there. 
%
% Input:
%
% 	X     		2p X N  N data points; one per column.
%			[x1, x2, x3, ..., xp, y1, y2, y3, ..., yp]';
%
% 	nu    		2p X 1  vector containing the mean 
%
% 	Omega 		2p X 2p	covariance matrix 
%
% 	refFeat		2 x 1	Number of the two reference features.
%
% Output:
%
% P    - (1  X N)  the probability density value
%
%
%  (c) in 1997 by California Institute of Technology
%
%  Markus Weber, Mike Burl
%

%
%  $Log$
%

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

% We need to compute shape variables in the standard shape space.
% This is equivalent to using the point (0,0) as the ref point for
% ref feature 1 and (1,0) as the ref point for ref feature 2.

stdref = zeros(2*p,1); stdref(refFeat(2)) = 1;
U = shape(X, stdref, refFeat(1), refFeat(2));


% Define the matrix L used to transform X to starred coords
fv = zeros(p,1);
fv(refFeat(1)) = 1;
Ipp = eye(p,p);
K0 = Ipp - ones(p,1) * fv';
L0 = [K0, zeros(p,p); zeros(p,p), K0]';
expunge = [~fv; ~fv]; % use as an index array to eliminate cols (or rows) f and f+p
L  = L0(:,expunge);   % expunge columns f and f+p

% Compute the mean and covariance in the starred coords
mu = L' * nu;
Sigma = L' * Omega * L;

%---------------------------------------------------------------------------------
% Precompute as many things as possible, so we don't repeat them in the loop

ISigma = inv(Sigma);
dSigma = det(Sigma);
Im     = ISigma * mu;
mIm    = mu' * Im;
i      = (0:p-2)';

pqa    = gamma(p-2+1) / ( sqrt(dSigma) * ((2*pi)^(p-2)) );

P = zeros(1, N);
for j = 1 : N
  uv     = [U(1:p,j), -U(p+1:2*p,j); U(p+1:2*p,j), U(1:p,j)];
  uvx    = uv(expunge,:); % expunge rows f and f+p from uv => uvx should = [u:v] in B-0002, eq 16,17
  IPsi   = uvx' * ISigma * uvx;

  [UI,SI,VI] = svd(IPsi,0);
  Phi     = UI;
  Psi     = UI * inv(SI) * UI';
  xi      = Psi * uvx' * Im;
  gg      = mIm - xi' * IPsi * xi;

  ss1 = 1/SI(2,2);
  ss2 = 1/SI(1,1);

  pqb = exp(-gg/2) * sqrt(ss1*ss2) * ((2*ss2)^(p-2));
  arg1 = -((Phi(:,1)' * xi)^2) / (2 * ss1);
  arg2 = -((Phi(:,2)' * xi)^2) / (2 * ss2);

  pqc = sum( (ss1/ss2).^i .* Laguerre(-0.5, i, arg1) .* Laguerre(-0.5, p-2-i, arg2) );

  P(j) = pqa * pqb * pqc;

end