void heapFinalize(PartialColoringHeap *heap, char* f, int l) {
heapClear(heap, f, l);
int count = heap->item_count;
if (count!=0) {
printf("Error in finalizing heap: %d items left\n", count);
assert(count!=0);
}
Free(heap->arrays);
Free(heap->size);
Free(heap->alloc_size);
}
int peFindPathWW(PED* PE, MapNode* Start, MapNode* End, peTime StartTime, int Speed, PathStep** path)
{
MapNode* v = Start;
MapNode* s;
MapNode* best = Start;
int i;
int BaseCost;
int c,z;
if (Start == End) return 0;
PE->LastSID++;
heapClear(PE->Open);
v->g = 0;
v->f = 0;
v->h = (*PE->Heuristics)(PE, v, End);
v->Steps = 0;
v->Time = 0;
heapAdd(PE->Open, v);
while ((v != End) && (PE->Open->ActSize!=0))
{
#ifdef PE_MONITOR // pro monitorovani enginu
peENodes++;
#endif
v = PE->Open->HeapBuffer[1];
heapRemove(PE->Open, v);
// best position is the closest to the dest
// and the most distanced from the start
if ((v->h<best->h) || (v->h=best->h) && (v->g>best->g)) best = v;
for (i=0; i<8; i++)
{
s = peMapXY(PE, v->Pos.X+Dir2Delta[i][0], v->Pos.Y+Dir2Delta[i][1]); // s = sousedni policko
if ((!s) || (s->TC==PE->MAXTC) || (s->Owner!=-1)) continue;
if (s->SearchID == PE->LastSID)
{
if (s->HeapPos>0)
{
if (DirType[i]==0)
BaseCost = PE->HorizontalCost;
else
BaseCost = PE->DiagonalCost;
c = v->g + BaseCost;
z = v->FootSteps;
if (c + z < s->g)
{
s->h = (*PE->Heuristics)(PE, s, End);
s->Steps = v->Steps + 1;
s->Time = v->Time + Speed;
s->f = (*PE->MixFunction)(PE, c, s->h, z);
s->g = c + z;
s->CameFrom = v->Pos;
heapSiftUp(PE->Open, s->HeapPos);
}
}
// else konecny
}
else
{
s->SearchID = PE->LastSID;
if (DirType[i]==0)
BaseCost = PE->HorizontalCost;
else
BaseCost = PE->DiagonalCost;
c = v->g + BaseCost;
z = v->FootSteps;
s->h = (*PE->Heuristics)(PE, s, End);
s->Steps = v->Steps + 1;
s->Time = v->Time + Speed;
s->f = (*PE->MixFunction)(PE, c, s->h, z);
s->g = c + z;
s->CameFrom = v->Pos;
if (heapAdd(PE->Open, s)==HP_NOSPACE)
{
// we ran out off free heap space
// get the best known position and continue
// after walker reaches this position
End = best;
v = best;
break;
}
}
}
}
if (Start == End) return 0;
if (v!=End) return 0;
if (!peAddPath(PE, path, End->Steps+1)) return 0;
i=v->Steps+1;
while (i>0)
{
i--;
(*path)[i].Time = StartTime + v->Time;
(*path)[i].Type = PST_CONTINUE | PST_FOOTSTEP;
(*path)[i].Pos = v->Pos;
v->FootSteps+=PE->FootStepCost;
v=peMapXY(PE, v->CameFrom.X, v->CameFrom.Y);
}
(*path)[End->Steps].Type = PST_END | PST_FOOTSTEP;
return End->Steps+1;
}