start to convert in cython

kdtree seam to work but no collision and links are integrated