Optimization of A*, better heuristic2, optimized Node