void ADPlanner::BuildNewOPENList(ADSearchStateSpace_t* pSearchStateSpace)
{
ADState *state;
CKey key;
CHeap* pheap = pSearchStateSpace->heap;
CList* pinconslist = pSearchStateSpace->inconslist;
//move incons into open
while (pinconslist->firstelement != NULL) {
state = (ADState*)pinconslist->firstelement->liststate;
//compute f-value
key = ComputeKey(state);
//insert into OPEN
if (state->heapindex == 0)
pheap->insertheap(state, key);
else
pheap->updateheap(state, key); //should never happen, but sometimes it does - somewhere there is a bug TODO
//remove from INCONS
pinconslist->remove(state, AD_INCONS_LIST_ID);
}
pSearchStateSpace->bRebuildOpenList = false;
}
void ARAPlanner::Reevaluatefvals(ARASearchStateSpace_t* pSearchStateSpace)
{
CKey key;
int i;
CHeap* pheap = pSearchStateSpace->heap;
//recompute priorities for states in OPEN and reorder it
for (i = 1; i <= pheap->currentsize; ++i) {
ARAState* state = (ARAState*)pheap->heap[i].heapstate;
pheap->heap[i].key.key[0] = state->g + (int)(pSearchStateSpace->eps * state->h);
//pheap->heap[i].key.key[1] = state->h;
}
pheap->makeheap();
pSearchStateSpace->bReevaluatefvals = false;
}
//note this does NOT re-compute heuristics, only re-orders OPEN list based on current eps and h-vals
void RSTARPlanner::Reevaluatefvals()
{
CKey key;
int i;
CHeap* pheap = pSearchStateSpace->OPEN;
//re-compute priorities for states in OPEN and reorder it
for (i = 1; i <= pheap->currentsize; ++i)
{
RSTARState* state = (RSTARState*)pheap->heap[i].heapstate;
pheap->heap[i].key = ComputeKey(state);
}
pheap->makeheap();
pSearchStateSpace->bReevaluatefvals = false;
}
void ADPlanner::Reevaluatefvals(ADSearchStateSpace_t* pSearchStateSpace)
{
CKey key;
int i;
CHeap* pheap = pSearchStateSpace->heap;
#if DEBUG
SBPL_FPRINTF(fDeb, "re-computing heap priorities\n");
#endif
//recompute priorities for states in OPEN and reorder it
for (i = 1; i <= pheap->currentsize; ++i) {
ADState* state = (ADState*)pheap->heap[i].heapstate;
pheap->heap[i].key = ComputeKey(state);
}
pheap->makeheap();
pSearchStateSpace->bReevaluatefvals = false;
}
void ADPlanner::BuildNewOPENList(ADSearchStateSpace_t* pSearchStateSpace)
{
ADState *state;
CKey key;
CHeap* pheap = pSearchStateSpace->heap;
CList* pinconslist = pSearchStateSpace->inconslist;
//move incons into open
while(pinconslist->firstelement != NULL)
{
state = (ADState*)pinconslist->firstelement->liststate;
//compute f-value
key = ComputeKey(state);
//insert into OPEN
pheap->insertheap(state, key);
//remove from INCONS
pinconslist->remove(state, AD_INCONS_LIST_ID);
}
}
void ARAPlanner::BuildNewOPENList(ARASearchStateSpace_t* pSearchStateSpace)
{
ARAState *state;
CKey key;
CHeap* pheap = pSearchStateSpace->heap;
CList* pinconslist = pSearchStateSpace->inconslist;
//move incons into open
while (pinconslist->firstelement != NULL) {
state = (ARAState*)pinconslist->firstelement->liststate;
//compute f-value
key.key[0] = state->g + (int)(pSearchStateSpace->eps * state->h);
//key.key[1] = state->h;
//insert into OPEN
pheap->insertheap(state, key);
//remove from INCONS
pinconslist->remove(state, ARA_INCONS_LIST_ID);
}
}
void TRAPlanner::Reevaluatefvals(TRASearchStateSpace_t* pSearchStateSpace)
{
CKey key;
int i;
CHeap* pheap = pSearchStateSpace->heap;
#if DEBUG
SBPL_FPRINTF(fDeb, "re-computing heap priorities\n");
#endif
//recompute priorities for states in OPEN and reorder it
for (i = 1; i <= pheap->currentsize; ++i)
{
TRAState* state = (TRAState*)pheap->heap[i].heapstate;
key.key[0] = state->g + (int)(pSearchStateSpace->eps*state->h);
pheap->heap[i].key = key;
}
pheap->makeheap();
pSearchStateSpace->bReevaluatefvals = false;
}
void anaPlanner::Reevaluatefvals(anaSearchStateSpace_t* pSearchStateSpace)
{
CKey key;
int i;
CHeap* pheap = pSearchStateSpace->heap;
//recompute priorities for states in OPEN and reorder it
for (i = 1; i <= pheap->currentsize; ++i) {
//anaState* state = (anaState*)pheap->heap[i].heapstate;
// CHANGED - cast removed
pheap->heap[i].key.key[0] = (long)-get_e_value(pSearchStateSpace,
((anaState*)pheap->heap[i].heapstate)->MDPstate->StateID);
//pheap->heap[i].key.key[1] = state->h;
}
pheap->makeheap();
pSearchStateSpace->bReevaluatefvals = false;
}
void CGameContext::ConRemoveVote(IConsole::IResult *pResult, void *pUserData)
{
CGameContext *pSelf = (CGameContext *)pUserData;
const char *pDescription = pResult->GetString(0);
// check for valid option
CVoteOptionServer *pOption = pSelf->m_pVoteOptionFirst;
while(pOption)
{
if(str_comp_nocase(pDescription, pOption->m_aDescription) == 0)
break;
pOption = pOption->m_pNext;
}
if(!pOption)
{
char aBuf[256];
str_format(aBuf, sizeof(aBuf), "option '%s' does not exist", pDescription);
pSelf->Console()->Print(IConsole::OUTPUT_LEVEL_STANDARD, "server", aBuf);
return;
}
// inform clients about removed option
CNetMsg_Sv_VoteOptionRemove OptionMsg;
OptionMsg.m_pDescription = pOption->m_aDescription;
pSelf->Server()->SendPackMsg(&OptionMsg, MSGFLAG_VITAL, -1);
// TODO: improve this
// remove the option
--pSelf->m_NumVoteOptions;
char aBuf[256];
str_format(aBuf, sizeof(aBuf), "removed option '%s' '%s'", pOption->m_aDescription, pOption->m_aCommand);
pSelf->Console()->Print(IConsole::OUTPUT_LEVEL_STANDARD, "server", aBuf);
CHeap *pVoteOptionHeap = new CHeap();
CVoteOptionServer *pVoteOptionFirst = 0;
CVoteOptionServer *pVoteOptionLast = 0;
int NumVoteOptions = pSelf->m_NumVoteOptions;
for(CVoteOptionServer *pSrc = pSelf->m_pVoteOptionFirst; pSrc; pSrc = pSrc->m_pNext)
{
if(pSrc == pOption)
continue;
// copy option
int Len = str_length(pSrc->m_aCommand);
CVoteOptionServer *pDst = (CVoteOptionServer *)pVoteOptionHeap->Allocate(sizeof(CVoteOptionServer) + Len);
pDst->m_pNext = 0;
pDst->m_pPrev = pVoteOptionLast;
if(pDst->m_pPrev)
pDst->m_pPrev->m_pNext = pDst;
pVoteOptionLast = pDst;
if(!pVoteOptionFirst)
pVoteOptionFirst = pDst;
str_copy(pDst->m_aDescription, pSrc->m_aDescription, sizeof(pDst->m_aDescription));
mem_copy(pDst->m_aCommand, pSrc->m_aCommand, Len+1);
}
// clean up
delete pSelf->m_pVoteOptionHeap;
pSelf->m_pVoteOptionHeap = pVoteOptionHeap;
pSelf->m_pVoteOptionFirst = pVoteOptionFirst;
pSelf->m_pVoteOptionLast = pVoteOptionLast;
pSelf->m_NumVoteOptions = NumVoteOptions;
}
bool anaPlanner::Search(anaSearchStateSpace_t* pSearchStateSpace, vector<int>& pathIds, int & PathCost,
bool bFirstSolution, bool bOptimalSolution, double MaxNumofSecs)
{
CKey key;
TimeStarted = clock();
searchexpands = 0;
#if DEBUG
fprintf(fDeb, "new search call (call number=%d)\n", pSearchStateSpace->callnumber);
#endif
if (pSearchStateSpace->bReinitializeSearchStateSpace == true) {
//re-initialize state space
ReInitializeSearchStateSpace(pSearchStateSpace);
}
if (bOptimalSolution) {
pSearchStateSpace->eps = 1;
MaxNumofSecs = INFINITECOST;
}
else if (bFirstSolution) {
MaxNumofSecs = INFINITECOST;
}
//ensure heuristics are up-to-date
environment_->EnsureHeuristicsUpdated((bforwardsearch == true));
//the main loop of ana*
int prevexpands = 0;
clock_t loop_time;
// CHANGE MADE TO WHILE LOOP to account for open.empty() == FALSE
while (!pSearchStateSpace->heap->emptyheap() && pSearchStateSpace->eps_satisfied > ana_FINAL_EPS &&
(clock() - TimeStarted) < MaxNumofSecs * (double)CLOCKS_PER_SEC)
{
loop_time = clock();
//decrease eps for all subsequent iterations
/*if(fabs(pSearchStateSpace->eps_satisfied - pSearchStateSpace->eps) < ERR_EPS && !bFirstSolution)
{
pSearchStateSpace->eps = pSearchStateSpace->eps - ana_DECREASE_EPS;
if(pSearchStateSpace->eps < ana_FINAL_EPS)
pSearchStateSpace->eps = ana_FINAL_EPS;
//the priorities need to be updated
pSearchStateSpace->bReevaluatefvals = true;
//it will be a new search
pSearchStateSpace->bNewSearchIteration = true;
//build a new open list by merging it with incons one
BuildNewOPENList(pSearchStateSpace);
}*/
pSearchStateSpace->searchiteration++;
pSearchStateSpace->bNewSearchIteration = false;
//re-compute f-values if necessary and reorder the heap
//if(pSearchStateSpace->bReevaluatefvals)
// Reevaluatefvals(pSearchStateSpace);
//improve or compute path
int retVal = ImprovePath(pSearchStateSpace, MaxNumofSecs);
anaState* state;
CKey key;
CHeap* open = pSearchStateSpace->heap;
//printf("states expanded: %d\t states considered: %d\t time elapsed: %f\n",searchexpands - prevexpands, pSearchStateSpace->heap->currentsize, double(clock() - TimeStarted)/CLOCKS_PER_SEC);
double epsprime = 1.0;
for (int j = 1; j <= open->currentsize;) {
state = (anaState*)open->heap[j].heapstate;
double temp_eps = (double)((pSearchStateSpace->G * 1.0) / (double)(state->g + state->h));
if (temp_eps > epsprime) {
epsprime = temp_eps;
}
double e_val = get_e_value(pSearchStateSpace, state->MDPstate->StateID);
if (e_val <= 1.0) {
open->deleteheap_unsafe(state);
}
else {
key.key[0] = (long)-e_val;
open->updateheap_unsafe(state, key);
++j;
}
pSearchStateSpace->eps_satisfied = epsprime;
}
open->makeheap();
//print the solution cost and eps bound
if (retVal == 1) {
//printf("suboptimality=%f expands=%d g(searchgoal)=%d loop_time=%.3f time_elapsed=%.3f memoryCounter=%d\n", pSearchStateSpace->eps_satisfied, searchexpands - prevexpands, ((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g,double(clock()-loop_time)/CLOCKS_PER_SEC, double(clock() - TimeStarted)/CLOCKS_PER_SEC, MaxMemoryCounter);
printf("suboptimality=%f g(searchgoal)=%d time_elapsed=%.3f memoryCounter=%d\n",
pSearchStateSpace->eps_satisfied,
((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g, double(clock()
- TimeStarted) / CLOCKS_PER_SEC, MaxMemoryCounter);
//printf("states expanded: %d\t states considered: %d\t time elapsed: %f\n",searchexpands - prevexpands, pSearchStateSpace->heap->currentsize, double(clock() - TimeStarted)/CLOCKS_PER_SEC);
}
#if DEBUG
fprintf(fDeb, "eps=%f expands=%d g(searchgoal)=%d time=%.3f\n", pSearchStateSpace->eps_satisfied, searchexpands - prevexpands,
((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g,double(clock()-loop_time)/CLOCKS_PER_SEC);
PrintSearchState((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData, fDeb);
#endif
prevexpands = searchexpands;
//if just the first solution then we are done
if (bFirstSolution) break;
//no solution exists
if (((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g == INFINITECOST) break;
}
#if DEBUG
fflush(fDeb);
#endif
printf("Suboptimality = %.4f\n", pSearchStateSpace->eps_satisfied);
PathCost = ((anaState*)pSearchStateSpace->searchgoalstate->PlannerSpecificData)->g;
MaxMemoryCounter += environment_->StateID2IndexMapping.size() * sizeof(int);
printf("MaxMemoryCounter = %d\n", MaxMemoryCounter);
int solcost = INFINITECOST;
bool ret = false;
if (PathCost == INFINITECOST) {
printf("could not find a solution\n");
ret = false;
}
else {
printf("solution is found\n");
pathIds = GetSearchPath(pSearchStateSpace, solcost);
ret = true;
}
printf("total expands this call = %d, planning time = %.3f secs, solution cost=%d\n", searchexpands, (clock()
- TimeStarted) / ((double)CLOCKS_PER_SEC), solcost);
//fprintf(fStat, "%d %d\n", searchexpands, solcost);
return ret;
}
