Working  thread KDTree that does not copy coordinates etc. No velocity functions implemented as yet