Found actual problem. The comparator in the KDTree had search first instead of current. All results now correct and it's about 500% faster now that the tree is navigated properly
Ok now checks distance between search and current not best and current. Works perfectly. Took me a while to track that one down. Previous solutions were incorrect.