Fast Kd-Tree lookup implementation (more accurately 1-d tree but you can modify the code to k-nearest easily, and currently building a tree is slow) using Eigen. Slow adhoc BVH implementation.

For those who can help themselves.