public static List<Vector4> Plan(Vector4 start, Vector4 target, Map m, int weight, bool canTimeTravel) { if (!canTimeTravel && target.w != start.w) { return new List<Vector4> (); } PQ pq = new PQ(); HashSet<Vector4> visited = new HashSet<Vector4>(); int target_x = m.getRowNumber(target.x); int target_y = m.getVerNumber(target.y); int target_z = m.getColNumber(target.z); int target_w = (int)target.w; int start_x = m.getRowNumber(start.x); int start_y = m.getVerNumber(start.y); int start_z = m.getColNumber(start.z); int start_w = (int)start.w; float eDist = Node.euclideanDistance(start_x, start_y, start_z, start_w, target_x, target_y, target_z, target_w); Node init = new Node(start_x, start_y, start_z, start_w, 0, eDist, new List<Vector4>()); pq.insert(init); int[][,,] inf_map = m.getInfluenceMapCopy (); Map.subtractInfluenceMap(start, weight, inf_map); while (!pq.isEmpty()) { Node n = pq.pop(); visited.Add(new Vector4(n.x, n.y, n.z, n.w)); if (n.x == target_x && n.y == target_y && n.z == target_z && n.w == target_w) { return n.path; } processNextActions(pq, visited, n, target_x, target_y, target_z, target_w, m, inf_map, canTimeTravel); } return new List<Vector4> (); }