Update FindOptimalCostPath.java