% function [xp_sorted] = sort_point_convex( xp )
% Sort points of a convex shape for correct shape drawing.
% In:
%   xp .. points (column) in a plane bounding a convex shape.
% Out:
%   xp_sorted .. sorted points
function [xp_sorted] = sort_point_convex( xp )
  
  nxp = size( xp, 2 );
  p0 = xp(:,1);

  vec(1,:) = xp(1,2:nxp) - p0(1);
  vec(2,:) = xp(2,2:nxp) - p0(2);
  vec(3,:) = xp(3,2:nxp) - p0(3);

  % Find point closest to p0.
  for i = 1:nxp-1
    nn(i) = norm( vec(:,i) );
  end
  [tmp, j] = min( nn );
%  j
  p1 = xp(:,j(1));
  for i = 1:nxp-1
    dv = (vec(:,j)' * vec(:,i));
    alfa(i) = acos( dv / nn(j) / nn(i) );
  end
%  alfa
  [alfa, perm] = sort( alfa );
%  alfa
%  perm
  xp_sorted = [p0 xp(:,perm+1)];
