int main(int argc, char **argv) {
int myints[] = {10, 20, 30, 5, 15};
Vector *v = NewVector(int, 5);
for (int i = 0; i < 5; i++) {
Vector_Push(v, myints[i]);
}
Make_Heap(v, 0, v->top, cmp);
int n;
Vector_Get(v, 0, &n);
assert(30 == n);
Heap_Pop(v, 0, v->top, cmp);
v->top = 4;
Vector_Get(v, 0, &n);
assert(20 == n);
Vector_Push(v, 99);
Heap_Push(v, 0, v->top, cmp);
Vector_Get(v, 0, &n);
assert(99 == n);
Vector_Free(v);
printf("PASS!\n");
return 0;
}
navpath_t* NavRep_PathCreate_Internal(navmesh_t* navmesh, const vec2_t src, navpoly_t* pathpoly_src, const vec2_t dest, navpoly_t* pathpoly_dest)
{
int ms = System_Milliseconds();
// increment the frame code (frame code is used in place of open/closed list)
int frame = ++navmesh->frame_cnxn;
if ( pathpoly_src == pathpoly_dest )
{
// if we're in the same pathpoly, just go there
navpathwaypt_t* waypoint;
path_last = (navpath_t*)ALLOCATE(navpath_t);
path_last->waypoints.first = NULL;
path_last->waypoints.last = NULL;
M_CopyVec2(dest, path_last->pos);
M_CopyVec2(dest, path_last->src);
M_CopyVec2(src, path_last->dest);
waypoint = NavPathWaypt_Create(src);
waypoint->bridge = pathpoly_src->bridge2 ? pathpoly_src : NULL;
List_PushFront(&path_last->waypoints, waypoint);
path_last->waypoint = path_last->waypoints.first;
// List_PushFront(&navpaths, path_last);
return path_last;
}
if ( !pathpoly_dest->bridge2 && NavMesh_Trace(navmesh, src, dest) )
{
// if we can trace, just go there
navpathwaypt_t* waypoint;
path_last = (navpath_t*)ALLOCATE(navpath_t);
path_last->waypoints.first = NULL;
path_last->waypoints.last = NULL;
M_CopyVec2(dest, path_last->pos);
M_CopyVec2(dest, path_last->src);
M_CopyVec2(src, path_last->dest);
waypoint = NavPathWaypt_Create(src);
List_PushFront(&path_last->waypoints, waypoint);
path_last->waypoint = path_last->waypoints.first;
// List_PushFront(&navpaths, path_last);
return path_last;
}
{
// create our heap
heap_t heap;
Heap_Init(&heap, 16384, Heap_Compare_PathPolyCnxn);
// push all initial cnxns onto heap
{
navpolyedge_t* edge = pathpoly_src->edges;
while ( edge )
{
navpolycnxn_t* cnxn = edge->cnxns.first;
while ( cnxn )
{
navpolygate_t* gate = cnxn->gates;
while ( gate )
{
// initialize costs
gate->distance = path_sqrt(M_GetDistanceSqVec2(src, gate->position));
gate->cost = gate->distance + path_sqrt(M_GetDistanceSqVec2(dest, gate->position));
gate->from = NULL;
gate->frame = frame;
// add to heap
Heap_Push(&heap, gate);
gate = gate->next;
}
cnxn = cnxn->next;
}
edge = edge->next;
}
}
while ( !Heap_IsEmpty(&heap) )
{
// extract min cost gate
navpolygate_t* gate = (navpolygate_t*)Heap_Pop(&heap);
if ( gate->cnxn->into == pathpoly_dest )
{
if ( fabs(gate->distance - gate->cost) < PATH_EPSILON )
{
// found a path, finalize it
path_last = NavRep_PathFinalize(navmesh, src, dest, gate);
NAV_PRINTF("NavRep_PathCreate succeeded, took %d milliseconds\n", System_Milliseconds() - ms);
// term the heap
Heap_Term(&heap);
// List_PushFront(&navpaths, path_last);
return path_last;
}
else
{
gate->distance += path_sqrt(M_GetDistanceSqVec2(gate->position, dest));
gate->cost = gate->distance;
Heap_Update(&heap, gate);
continue;
}
}
// relax all adjacent gates
{
navpoly_t* pathpoly = gate->cnxn->into;
navpolyedge_t* pathpoly_edge = pathpoly->edges;
while ( pathpoly_edge )
{
navpolycnxn_t* pathpoly_cnxn = pathpoly_edge->cnxns.first;
while ( pathpoly_cnxn )
{
navpolygate_t* pathpoly_gate = pathpoly_cnxn->gates;
while ( pathpoly_gate )
{
float distance = 1.0f + gate->distance + path_sqrt(M_GetDistanceSqVec2(pathpoly_gate->position, gate->position));
float cost = distance + path_sqrt(M_GetDistanceSqVec2(pathpoly_gate->position, dest));
if ( pathpoly_gate->frame != frame )
{
pathpoly_gate->cost = cost;
pathpoly_gate->distance = distance;
pathpoly_gate->frame = frame;
pathpoly_gate->from = gate;
Heap_Push(&heap, pathpoly_gate);
}
// $todo: why is it okay (faster + just as accurate) to compare distance here rather than cost????
else if ( (pathpoly_gate->distance > distance) )
{
pathpoly_gate->cost = cost;
pathpoly_gate->distance = distance;
pathpoly_gate->from = gate;
Heap_Update(&heap, pathpoly_gate);
}
pathpoly_gate = pathpoly_gate->next;
}
pathpoly_cnxn = pathpoly_cnxn->next;
}
pathpoly_edge = pathpoly_edge->next;
}
}
}
NAV_PRINTF("NavRep_PathCreate failed, took %d milliseconds\n", System_Milliseconds() - ms);
// term the heap
Heap_Term(&heap);
}
return NULL;
}
