void step() { expand_frontier(); frontier_pop(); print_path(); }
std::vector<PState*> Astar::run(int* start, double* target){ // std::cout << "Running A*\n"; // std::cout << "Initializing target\n"; this->target = target; // std::cout << "Initialize Start\n"; compute_fk(start[0], start[1], start[2]); State* starts = &space[start[0]][start[1]][start[2]]; starts->heuristic = heuristic(&space[start[0]][start[1]][start[2]]); starts->prev[0] = -1; starts->prev[1] = -1; starts->prev[2] = -1; starts->value = starts->heuristic; // std::cout << "Initialzing Priority Queue\n"; PState* pstart = new PState; pstart->value = starts->value; pstart->state[0] = start[0]; pstart->state[1] = start[1]; pstart->state[2] = start[2]; frontier.push(pstart); State* current = starts; PState* tracker = new PState; // std::cout << "Searching For Target\n"; while(!frontier.empty() && will_continue(current)){ /* Get the next priority */ PState* pcur = frontier.top(); frontier.pop(); tracker->value = pcur->value; tracker->state[0] = pcur->state[0]; tracker->state[1] = pcur->state[1]; tracker->state[2] = pcur->state[2]; current = &space[pcur->state[0]][pcur->state[1]][pcur->state[2]]; if(current->visited == false){ current->visited = true; expand_frontier(pcur->state[0],pcur->state[1],pcur->state[2]); } delete(pcur); } // std::cout << "Search Completed\n"; std::vector<PState*> path; State* back_tracker = current;; path.push_back(tracker); // std::cout <<"Compiling Path\n"; while(back_tracker != starts){ int i0 = back_tracker->prev[0]; int i1 = back_tracker->prev[1]; int i2 = back_tracker->prev[2]; PState* next = new PState; next->value = back_tracker->value; next->state[0] = i0; next->state[1] = i1; next->state[2] = i2; back_tracker = &space[i0][i1][i2]; path.push_back(next); } return path; }
std::vector<configState*> Astar::run(configState* s, wsState* t){ std::cout << "Initializing Start and Target\n"; target = t; start = s; std::cout << "Target: "; wsstate_tostring(target); std::cout << "Initialize start data structures\n"; /* Runtime checks */ float avg_frontier_pop = 0; int pop_count = 0; float avg_vis_add = 0; int vis_add_count = 0; float avg_expansion = 0; int expansion_count = 0; /* Init */ visData* startv = create_visdata(); startv->current = start; startv->prev = 0; startv->value = heuristic(start); frontier.push(startv); visData* next = 0; wsState* wscurrent = fk(start); while(distance(target, wscurrent) > target_threshold){ clock_t pqstart = clock(); next = frontier.top(); frontier.pop(); float secsElapsed = (float)(clock() - pqstart)/CLOCKS_PER_SEC; avg_frontier_pop += secsElapsed; pop_count++; delete(wscurrent); wscurrent = fk(next->current); if(next != 0){ clock_t visaddstart = clock(); add_visited(next->current, next); secsElapsed= (float)(clock() - visaddstart)/CLOCKS_PER_SEC; avg_vis_add += secsElapsed; vis_add_count ++; clock_t expstart = clock(); expand_frontier(next->current); secsElapsed = (float)(clock() - expstart)/CLOCKS_PER_SEC; avg_expansion += secsElapsed; expansion_count++; } else { std::vector<configState*> path; return path; } } std::cout << "Visited Set: "<< visited_set.size() << '\n'; std::cout << "Queue: "<< frontier.size() << '\n'; std::cout << "Completed Search\n"; std::cout << "Runtimes: \n\n\n\n"; std::cout << "Frontier Popping :" << avg_frontier_pop << '\n'; std::cout << "Expansion: " << avg_expansion << '\n'; std::cout << " Vis Gets: " << get_visdata_time << '\n'; std::cout << " Value Comp Time: " << value_comp_time << '\n'; std::cout << " Frontier Push Time: " << frontier_insert_time << '\n'; std::cout << " Mem_Time: " << mem_time << '\n'; std::cout << " VisAdd :" << avg_vis_add*3 << '\n'; std::cout << "Counts: " << pop_count << '\n'; visData* current = next; std::vector<configState*> path; std::cout << "Backtracing path\n"; while(current != 0){ path.push_back(current->current); if(current -> prev == 0){ current = 0; continue; } current = get_visdata(current->prev); } std::cout << "Back trace completed\n"; return path; }