Implement coherent point drift algorithm compiling with C and matlab, this is an official version, do not change it.