KDTree works!

Probably can to be optimised. Need to compare its speed against a linear
search for different mesh complexities.