Started work on kd-tree. May be a good idea to have some tests for certain classes and functions.