kdtree implementation

Signed-off-by: xSilencer08 <fwehlus@gmail.com>