void CPathFind::FindChildPathSub(_PathNode *node, int x, int y, int dx, int dy, int arg) { int g, c=0; _PathNode *old_node,*t_node; g = node->g + arg; if((old_node = CheckOpen(x, y)) != NULL) { for(c = 0; c < 8; c++) { if(node->Child[c] == NULL) { break; } } node->Child[c] = old_node; if(g < old_node->g) { old_node->Parent = node; old_node->g = g; old_node->f = g + old_node->h; } } else if((old_node = CheckClosed(x, y)) != NULL) { for(c = 0; c < 8; c++) { if(node->Child[c] == NULL) { break; } } node->Child[c] = old_node; if(g < old_node->g) { old_node->Parent = node; old_node->g = g; old_node->f = g + old_node->h; PropagateDown(old_node); } } else { t_node = (_PathNode *)calloc(1, sizeof(_PathNode)); t_node->Parent = node; t_node->g = g; // t_node->h = (int)sqrt((x-dx)*(x-dx) + (y-dy)*(y-dy)); t_node->h = (int)max( x-dx, y-dy ); t_node->f = g + t_node->h; t_node->x = x; t_node->y = y; Insert(t_node); for(c = 0; c < 8; c++) { if(node->Child[c] == NULL) { break; } } node->Child[c] = t_node; } }
//=========================================== // Successor Nodes all pushed onto OPEN list //=========================================== void GetSuccessorNodes(node_t *StartNode, int NodeNumS, int NodeNumD) { node_t *Old,*Successor; node_t *tNode1,*tNode2; int g,c; float h; // NOTE: NodeNumS is the index of a node that was found by the node searching routine //================================ // Has NodeNumS been Searched yet? //================================ // see if this node is already on the OPEN list Old = CheckLIST(OPEN, NodeNumS); if (Old) { // node was found on the OPEN list // this means the node was found before (as a child of another node) // but not yet searched (as a parent node) for (c = 0; c < NUMCHILDS; c++) { // break on the first available child slot of StartNode if (!StartNode->Child[c]) break; } // if we found an empty child slot, use it, otherwise use the last one StartNode->Child[((c < NUMCHILDS)?c:(NUMCHILDS-1))] = Old; // have we gone farther with this node than StartNode? if (StartNode->g + 1 < Old->g) { Old->g = g = StartNode->g + 1; // make node one step beyond StartNode Old->f = g + Old->h; // update total cost Old->PrevNode = StartNode; // reverse link to StartNode } return; } //================================== // Has NodeNumS been searched yet? //================================== Old=CheckLIST(CLOSED,NodeNumS); if (Old!=NULL) { // node has been searched before for (c=0;c < NUMCHILDS;c++) if (StartNode->Child[c]==NULL) break; StartNode->Child[((c < NUMCHILDS)?c:(NUMCHILDS-1))]=Old; if (StartNode->g+1 < Old->g) { Old->g=g=StartNode->g+1; Old->f=g+Old->h; Old->PrevNode=StartNode; PropagateDown(Old); } return; } //======================================= // It is NOT on the OPEN or CLOSED List!! //======================================= // Make Successor a Child of StartNode //======================================= //Successor=(node_t *)malloc(sizeof(node_t)); Successor=(node_t *)V_Malloc(sizeof(node_t), TAG_LEVEL); Successor->nodenum=NodeNumS; Successor->g=g=StartNode->g+1; //GHz - track node memory use so we can free this later // NodeList[NodeCount++] = Successor; NodeCount++; // NOTE: the heuristic estimate of the remaining path from this node // to the destination node is given by the difference between the 2 // vectors. You can come up with your own estimate.. Successor->h=h=distance(pathnode[NodeNumS], pathnode[NodeNumD]);//fabs(vDiff(node[NodeNumS].origin,node[NodeNumD].origin));//GHz - changed to fabs() Successor->f=g+h; Successor->PrevNode=StartNode; // reverse link to StartNode Successor->NextNode=NULL; // make all child links of new Successor node NULL for (c=0;c < NUMCHILDS;c++) Successor->Child[c]=NULL; for (c=0;c < NUMCHILDS;c++) if (StartNode->Child[c]==NULL) break; // Find first empty Child[] of StartNode StartNode->Child[((c < NUMCHILDS)?c:(NUMCHILDS-1))]=Successor; // make Successor a child of StartNode //================================= // Insert Successor into OPEN List //================================= tNode1=OPEN; tNode2=OPEN->NextNode; // find node in OPEN list with f-cost greater than Successor node while (tNode2 && (tNode2->f < Successor->f)) { tNode1=tNode2; tNode2=tNode2->NextNode; } Successor->NextNode=tNode2; tNode1->NextNode=Successor; //gi.dprintf("added node %d to the OPEN list\n", Successor->nodenum); }