A* calcMovementCost() speed fix