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