Fix memoryleak during kdtree usage.

Signed-off-by: TheSniperFan <thesniperfan@gmail.com>
Use vectors for neighbors.
Some documentation work.

Signed-off-by: TheSniperFan <thesniperfan@gmail.com>