CPP - AStar - Minor optimization and function scope mod.