MSP: Track the KDTree size
MSP: Reserve paths vector in advance
MSP: Replace shared_ptr usage by standard pointers