Add KD-Tree

Already write it once but haven't put it on team notebook :)