Copy of http://www.mathworks.com/matlabcentral/fileexchange/41396-nonrigidicp