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;
}
