Merged newest MathGeoLib KdTree code. Closes #670.