optimized func for A*