/*------------------------------------------------------------------
* Function: Iterative_dfs
* Purpose: Use a stack variable to implement an iterative version
* of depth-first search
* In arg:
* tour: partial tour of cities visited so far (just city 0)
* Globals in:
* n: total number of cities in the problem
* Notes:
* 1 The input tour is modified during execution of search,
* but returned to its original state before returning.
* 2. The Update_best_tour function will modify the global var
* best_tour
*/
void Iterative_dfs(tour_t tour) {
city_t nbr;
my_stack_t stack;
tour_t curr_tour;
stack = Init_stack();
Push(stack, tour);
while (!Empty(stack)) {
curr_tour = Pop(stack);
# ifdef DEBUG
printf("Popped tour = %p and %p\n", curr_tour, curr_tour->cities);
Print_tour(curr_tour, "Popped");
printf("\n");
# endif
if (City_count(curr_tour) == n) {
if (Best_tour(curr_tour))
Update_best_tour(curr_tour);
} else {
for (nbr = n-1; nbr >= 1; nbr--)
if (Feasible(curr_tour, nbr)) {
Add_city(curr_tour, nbr);
Push(stack, curr_tour);
Remove_last_city(curr_tour);
}
}
Free_tour(curr_tour);
}
Free_stack(stack);
} /* Iterative_dfs */
/*------------------------------------------------------------------
* Function: Search
* Purpose: Search for an optimal tour
* Global vars in: mat, n
* Global vars in/out: best_tour
*/
void *Search(void* rank) {
city_t nbr;
city_t city;
weight_t cost;
weight_t local_best_cost = best_tour.cost;
tour_t* tour_p;
stack_elt_t* stack_p = NULL;
stack_elt_t* temp_p;
long my_rank = (long) rank;
int quotient, leftovers, i;
int partial_tour_count, first_final_city, last_final_city;
quotient = (n-1) / thread_count;
leftovers = (n-1) % thread_count;
if(my_rank < leftovers){
partial_tour_count = quotient+1;
first_final_city = my_rank*partial_tour_count + 1;
}
else{
partial_tour_count = quotient;
first_final_city = my_rank*partial_tour_count + leftovers + 1;
}
last_final_city = first_final_city + partial_tour_count - 1;
for(i = first_final_city; i <= last_final_city; i++){
tour_p = malloc(sizeof(tour_t));
Initialize_tour(tour_p);
tour_p->cities[tour_p->count] = 0;
tour_p->count++;
tour_p->cost = 0;
temp_p = malloc(sizeof(stack_elt_t));
temp_p->tour_p = tour_p;
temp_p->city = i;
temp_p->cost = mat[i];
temp_p->next_p = stack_p;
stack_p = temp_p;
}
while (!Empty(stack_p)) {
Pop(&tour_p, &city, &cost, &stack_p);
tour_p->cities[tour_p->count] = city;
tour_p->cost += cost;
tour_p->count++;
if (tour_p->count == n) {
if (tour_p->cost + mat[city*n + 0] < local_best_cost){
Check_best_tour(city, tour_p, &local_best_cost);
}
} else {
for (nbr = n-1; nbr > 0; nbr--) {
pthread_mutex_lock(&mutex);
if (Feasible(city, nbr, tour_p, local_best_cost))
Push(tour_p, nbr, mat[n*city+nbr], &stack_p);
pthread_mutex_unlock(&mutex);
}
}
/* Push duplicates the tour. So it needs to be freed */
free(tour_p->cities);
free(tour_p);
} /* while */
return NULL;
} /* Search */
vector<Node*> Pendulum::FindPathIterations(int iterations) {
cout << "starting path iteration!\n";
vector<Node*> goal_nodes;
double min_cost = DBL_MAX;
int sol = 0;
for (int iter = 0; iter < iterations; iter++){
if(iter%5000==0){cout<<"iterations:"<<iter<<endl;}
//cout << sol << endl;
/**/
Node* prev = GetNodeToExpandRRT();
double u =((double)rand() / RAND_MAX - 0.5)*max_u;
Node* n = d.update(prev, u);
/*Test-Node generation
double testx = 2;
double testtheta = .1;
vector<double> times;
times.push_back(0);
times.push_back(0);
state_type s1;
s1.push_back(3);
s1.push_back(0);
s1.push_back(0);
s1.push_back(0);
state_type s2;
s2.push_back(40);
s2.push_back(0);
s2.push_back(2*PI-0.1);
s2.push_back(0);
vector<state_type> trajectory;
trajectory.push_back(s1);
trajectory.push_back(s2);
Node* n = new Node(0, testx, 0, testtheta, 0, 0, 0, NULL, times, trajectory);
n->print_node();
cout << Feasible(n) << endl;
*/
/**/
while (!Feasible(n)){
//n->print_node();
//cout << "found unfeasible node!\n";
int baditer = 1;
if(baditer%5000==0){cout<<"bad iterations:"<<baditer<<endl;}
delete n;
prev = GetNodeToExpandRRT();
u =((double)rand() / RAND_MAX - 0.5)*max_u;
n = d.update(prev, u);
}
prev->add_child(n);
//n->print_node();
if (CheckGoal(n)){
if (n->cost < min_cost){
min_cost = n->cost;
goal_nodes.push_back(n);
cout << "found better goal node! " << n->cost << endl;
sol = sol+1;
} /*
else{
cout << "found less optimal goal node! " << n->cost << endl;
}
*/
//Time and work are strictly positive
} else {
double pt[4] = {n->x*10.0/(bounds->ux-bounds->lx), n->v*10.0/(2*bounds->max_v)+5.0, n->theta*10.0/(6.28318530718), n->w*10.0/(2*bounds->max_w)+5.0};
if (kd_insert(nodes_tree, pt, n) != 0) {
cout << "didn't insert root successfully\n";
}
nodes.push_back(n);
}
}
cout << "finished iterating!\n";
cout << "Number of solutions found: " << sol << endl;
#ifdef OUTNEAREST
if (sol < 1){
double pt[4] = {b->x*10.0/(bounds->ux-bounds->lx), b->v*10.0/(2*bounds->max_v)+5.0, b->theta*10.0/(6.28318530718), b->w*10.0/(2*bounds->max_w)+5.0};
goalnodes.push_back()
}
