std::list<STATE> AStar(const STATE& start, const STATE& target, boost::function<void (const STATE&, const STATE&, const STATE&, std::vector<STATE>&, std::vector<STATE>&)> generate_next_states) { //std::cout<<"start A* ------------------"<<std::endl; std::set<STATE> open; std::set<STATE> closed; STATE current = start; while ( current.moreCost() > 0.0000001f ){ //std::cout<<"state: "<<current.p()<<std::endl; typename std::set<STATE>::iterator iter = closed.begin(); for(; closed.end() != iter; ++iter){ if ( iter->sameAs(current) ) break; } if ( closed.end() == iter ){ // insert to closed closed.insert(current); } iter = open.begin(); for(; open.end() != iter; ++iter){ if (iter->sameAs(current)){ open.erase(iter); break; } } std::vector<STATE> nextStates, unReach; generate_next_states(current, target, target, nextStates, unReach); //std::cout<<"next size: "<<nextStates.size()<<std::endl; for( typename std::vector<STATE>::iterator iter=nextStates.begin(); nextStates.end()!=iter; ++iter){ typename std::set<STATE>::iterator citer = closed.begin(); for(; citer != closed.end(); ++citer){ if ( citer->sameAs(*iter) ) break; } if ( closed.end() != citer ){ //std::cout<<"the state "<<iter->p() //<<"have been closed"<<std::endl; continue; // the state have be closed } iter->setPrevious(current); typename std::set<STATE>::iterator oiter = open.begin(); for(; oiter != open.end(); ++oiter){ if ( oiter->sameAs(*iter) ) break; } if ( open.end() == oiter ){ //std::cout<<"insert "<<iter->p() //<<" ("<<iter->cost()<<")"<<std::endl; open.insert(*iter); // first reach this state } else{ if ( oiter->cost() > iter->cost() ){ //std::cout<<"refresh "<<iter->p() //<<" "<<oiter->cost()<<" -> " //<<iter->cost()<<std::endl; // find a better path to reach this state open.erase(oiter); open.insert(*iter); } } } if ( open.empty() ){ //std::cout<<"no path! open empty"<<std::endl; break; // no path! } // start from the best state at currently current = *open.begin(); } std::list<STATE> path; while ( !start.sameAs(current) ) { path.push_front(current); STATE previous = current.previous(); typename std::set<STATE>::const_iterator p = closed.begin(); for(; p!=closed.end(); ++p ){ if ( previous.sameAs(*p) ) break; } if ( closed.end() == p ){ //std::cout<<"no path! closed empty"<<std::endl; // no path: closed empty! break; } current = *p; } //std::cout<<"end A* ----------------"<<std::endl; return path; }