Search fuction optimized and distance between points added