/*------------------------------------------------------------------ * 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() }