void PedestrianDirectionsEngine::Generate(IRoadGraph const & graph, vector<Junction> const & path,
Route::TTimes & times, Route::TTurns & turns,
vector<Junction> & routeGeometry,
my::Cancellable const & cancellable)
{
times.clear();
turns.clear();
routeGeometry.clear();
if (path.size() <= 1)
return;
CalculateTimes(graph, path, times);
vector<Edge> routeEdges;
if (!ReconstructPath(graph, path, routeEdges, cancellable))
{
LOG(LDEBUG, ("Couldn't reconstruct path."));
// use only "arrival" direction
turns.emplace_back(path.size() - 1, turns::PedestrianDirection::ReachedYourDestination);
return;
}
CalculateTurns(graph, routeEdges, turns, cancellable);
routeGeometry = path;
}
vector<int> ADPlanner::GetSearchPath(ADSearchStateSpace_t* pSearchStateSpace, int& solcost)
{
vector<int> SuccIDV;
vector<int> CostV;
vector<int> wholePathIds;
ADState* searchstateinfo;
CMDPSTATE* state = NULL;
CMDPSTATE* goalstate = NULL;
CMDPSTATE* startstate=NULL;
#if AD_SEARCH_FORWARD == 1
startstate = pSearchStateSpace->searchstartstate;
goalstate = pSearchStateSpace->searchgoalstate;
//reconstruct the path by setting bestnextstate pointers appropriately
if(ReconstructPath(pSearchStateSpace) != 1){
solcost = INFINITECOST;
return wholePathIds;
}
#else
startstate = pSearchStateSpace->searchgoalstate;
goalstate = pSearchStateSpace->searchstartstate;
#endif
#if DEBUG
PrintSearchPath(pSearchStateSpace, fDeb);
#endif
state = startstate;
wholePathIds.push_back(state->StateID);
solcost = 0;
FILE* fOut = stdout;
int steps = 0;
const int max_steps = 10000;
while(state->StateID != goalstate->StateID && steps < max_steps)
{
steps++;
if(state->PlannerSpecificData == NULL)
{
fprintf(fOut, "path does not exist since search data does not exist\n");
break;
}
searchstateinfo = (ADState*)state->PlannerSpecificData;
if(searchstateinfo->bestnextstate == NULL)
{
fprintf(fOut, "path does not exist since bestnextstate == NULL\n");
break;
}
if(searchstateinfo->g == INFINITECOST)
{
fprintf(fOut, "path does not exist since bestnextstate == NULL\n");
break;
}
environment_->GetSuccs(state->StateID, &SuccIDV, &CostV);
int actioncost = INFINITECOST;
for(int i = 0; i < (int)SuccIDV.size(); i++)
{
if(SuccIDV.at(i) == searchstateinfo->bestnextstate->StateID)
actioncost = CostV.at(i);
}
solcost += actioncost;
//fprintf(fDeb, "actioncost=%d between states %d and %d\n",
// actioncost, state->StateID, searchstateinfo->bestnextstate->StateID);
//environment_->PrintState(state->StateID, false, fDeb);
//environment_->PrintState(searchstateinfo->bestnextstate->StateID, false, fDeb);
state = searchstateinfo->bestnextstate;
wholePathIds.push_back(state->StateID);
}
if(state->StateID != goalstate->StateID){
printf("ERROR: Failed to getsearchpath, steps processed=%d\n", steps);
wholePathIds.clear();
solcost = INFINITECOST;
return wholePathIds;
}
return wholePathIds;
}
