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; }