/// Path_node processing in A* pathfinding. /// Adds new node to heap and updates/re-adds old ones if necessary. static int add_path(struct node_heap *heap, struct path_node *tp, int16 x, int16 y, int g_cost, struct path_node *parent, int h_cost) { int i = calc_index(x, y); if (tp[i].x == x && tp[i].y == y) { // We processed this node before if (g_cost < tp[i].g_cost) { // New path to this node is better than old one // Update costs and parent tp[i].g_cost = g_cost; tp[i].parent = parent; tp[i].f_cost = g_cost + h_cost; if (tp[i].flag == SET_CLOSED) { heap_push_node(heap, &tp[i]); // Put it in open set again } else if (heap_update_node(heap, &tp[i])) { return 1; } tp[i].flag = SET_OPEN; } return 0; } if (tp[i].x || tp[i].y) // Index is already taken; see `tp` array FIXME for details return 1; // New node tp[i].x = x; tp[i].y = y; tp[i].g_cost = g_cost; tp[i].parent = parent; tp[i].f_cost = g_cost + h_cost; tp[i].flag = SET_OPEN; heap_push_node(heap, &tp[i]); return 0; }
/* Loops the index over all points. Returns 0 when no more */ int next_index3(bindex *bx) { bx->jz++; if(bx->jz >= ncz) { bx->jz = 0; bx->jy++; if(bx->jy > jend) { bx->jy =jstart; bx->jx++; if((bx->region == RGN_NOBNDRY) || (bx->region == RGN_NOX)) { // Missing out X boundary if(bx->jx >= (ngx-MXG)) { bx->jx = MXG; return(0); } }else { // Including X boundary regions if(bx->jx >= ngx) { bx->jx = 0; return(0); } } } } calc_index(bx); return(1); }
/*========================================== * attach/adjust path if neccessary *------------------------------------------*/ static int add_path(int *heap,struct tmp_path *tp,int16 x,int16 y,int dist,int before,int cost) { int i; i = calc_index(x,y); if( tp[i].x == x && tp[i].y == y ) { if( tp[i].dist > dist ) { tp[i].dist = dist; tp[i].before = before; tp[i].cost = cost; if( tp[i].flag ) push_heap_path(heap,tp,i); else update_heap_path(heap,tp,i); tp[i].flag = 0; } return 0; } if( tp[i].x || tp[i].y ) return 1; tp[i].x = x; tp[i].y = y; tp[i].dist = dist; tp[i].before = before; tp[i].cost = cost; tp[i].flag = 0; push_heap_path(heap,tp,i); return 0; }
/*! Mark that block free and register it in the free index table. @param target Pointer to target block. */ static void add_free_block(FREE_BLOCK *target) { target->f = FLAG_FREE_BLOCK; int index = calc_index(target->size) - 1; int fli = FLI(index); int sli = SLI(index); free_fli_bitmap |= (MSB_BIT1 >> fli); free_sli_bitmap[fli] |= (MSB_BIT1 >> sli); target->prev_free = NULL; target->next_free = free_blocks[index]; if( target->next_free != NULL ) { target->next_free->prev_free = target; } free_blocks[index] = target; #ifdef MRBC_DEBUG target->vm_id = UINT8_MAX; memset( (uint8_t *)target + sizeof(FREE_BLOCK), 0xff, target->size - sizeof(FREE_BLOCK) ); #endif }
bool find (int_type const & id) { std::size_t index = calc_index(id); boost::mutex::scoped_lock lock (ref_mutex[index]); return data[index].find(id) != data[index].end(); }
bool insert(int_type const & id) { std::size_t index = calc_index(id); boost::mutex::scoped_lock lock (ref_mutex[index]); std::pair<typename std::set<int_type>::iterator, bool> p; p = data[index].insert(id); return p.second; }
void threaded_listener::disconnected(std::string const &to) throw (std::exception) { if (!empty()) { unsigned int index = calc_index(to); queue_item_type item; item.to = to; item.id = 0; items_[index]->push(item); } }
/*! just remove the free_block *target from index @param target pointer to target block. */ static void remove_index(FREE_BLOCK *target) { // top of linked list? if( target->prev_free == NULL ) { int index = calc_index(target->size) - 1; free_blocks[index] = target->next_free; if( free_blocks[index] == NULL ) { int fli = FLI(index); int sli = SLI(index); free_sli_bitmap[fli] &= ~(MSB_BIT1 >> sli); if( free_sli_bitmap[fli] == 0 ) free_fli_bitmap &= ~(MSB_BIT1 >> fli); }
bool erase(int_type const & id) { std::size_t index = calc_index(id); boost::mutex::scoped_lock lock (ref_mutex[index]); if (data[index].find(id) != data[index].end()) { data[index].erase(id); assert(data[index].find(id) == data[index].end()); return true; } else return false; }
/* Resets the index bx */ void start_index(bindex *bx, REGION region) { // Initialize it to something bx->jx = 0; if((region == RGN_NOBNDRY) || (region == RGN_NOX)) bx->jx = MXG; bx->jy = jstart; bx->jz = 0; bx->region = region; calc_index(bx); }
void threaded_listener::connection_closed_ex( std::string const &to, globals::connection_id id, boost::shared_ptr<guard> const ¬ify_guard) throw (std::exception) { assert(NULL != notify_guard.get()); if (!empty()) { unsigned int index = calc_index(to); queue_item_type item; item.to = to; item.id = id; item.notify_guard = notify_guard; item.for_open = false; items_[index]->push(item); } }
/* Loops over all perpendicular indices (no Y) */ int next_indexperp(bindex *bx) { bx->jz++; if(bx->jz >= ngz) { bx->jz = 0; bx->jx++; if(bx->jx >= (ngx-MXG)) { bx->jx = MXG; return(0); } } calc_index(bx); return(1); }
/*========================================== * 必要ならpathを追加/修正する *------------------------------------------ */ static int add_path(int *heap, struct tmp_path *tp, int x, int y, int dist, DIR dir, int before, int x1, int y1) { int i; nullpo_ret(heap); nullpo_ret(tp); i = calc_index(x, y); if (tp[i].x == x && tp[i].y == y) { if (tp[i].dist > dist) { tp[i].dist = dist; tp[i].dir = dir; tp[i].before = before; tp[i].cost = calc_cost(&tp[i], x1, y1); if (tp[i].flag) push_heap_path(heap, tp, i); else update_heap_path(heap, tp, i); tp[i].flag = 0; } return 0; } if (tp[i].x || tp[i].y) return 1; tp[i].x = x; tp[i].y = y; tp[i].dist = dist; tp[i].dir = dir; tp[i].before = before; tp[i].cost = calc_cost(&tp[i], x1, y1); tp[i].flag = 0; push_heap_path(heap, tp, i); return 0; }
/* Loops the index over 2D domain (no Z) */ int next_index2(bindex *bx) { bx->jy++; if(bx->jy > jend) { bx->jy =jstart; bx->jx++; if((bx->region == RGN_NOBNDRY) || (bx->region == RGN_NOX)) { if(bx->jx >= (ngx-MXG)) { bx->jx = MXG; return(0); } }else if(bx->jx >= ngx) { bx->jx = 0; return(0); } } calc_index(bx); return(1); }
/*========================================== * path search (x0,y0)->(x1,y1) * wpd: path info will be written here * flag: &1 = easy path search only * cell: type of obstruction to check for *------------------------------------------*/ bool path_search (struct walkpath_data *wpd, int m, int x0, int y0, int x1, int y1, int flag, cell_chk cell) { int heap[MAX_HEAP + 1]; struct tmp_path tp[MAX_WALKPATH * MAX_WALKPATH]; register int i, j, len, x, y, dx, dy; int rp, xs, ys; struct map_data *md; struct walkpath_data s_wpd; if (wpd == NULL) wpd = &s_wpd; // use dummy output variable if (!map[m].cell) return false; md = &map[m]; #ifdef CELL_NOSTACK //Do not check starting cell as that would get you stuck. if (x0 < 0 || x0 >= md->xs || y0 < 0 || y0 >= md->ys) #else if (x0 < 0 || x0 >= md->xs || y0 < 0 || y0 >= md->ys /*|| map_getcellp(md,x0,y0,cell)*/) #endif return false; if (x1 < 0 || x1 >= md->xs || y1 < 0 || y1 >= md->ys || map_getcellp (md, x1, y1, cell)) return false; // calculate (sgn(x1-x0), sgn(y1-y0)) dx = ( (dx = x1 - x0)) ? ( (dx < 0) ? -1 : 1) : 0; dy = ( (dy = y1 - y0)) ? ( (dy < 0) ? -1 : 1) : 0; // try finding direct path to target x = x0; y = y0; i = 0; while (i < ARRAYLENGTH (wpd->path)) { wpd->path[i] = walk_choices[-dy + 1][dx + 1]; i++; x += dx; y += dy; if (x == x1) dx = 0; if (y == y1) dy = 0; if (dx == 0 && dy == 0) break; // success if (map_getcellp (md, x, y, cell)) break; // obstacle = failure } if (x == x1 && y == y1) { //easy path successful. wpd->path_len = i; wpd->path_pos = 0; return true; } if (flag & 1) return false; memset (tp, 0, sizeof (tp)); i = calc_index (x0, y0); tp[i].x = x0; tp[i].y = y0; tp[i].dist = 0; tp[i].before = 0; tp[i].cost = calc_cost (&tp[i], x1, y1); tp[i].flag = 0; heap[0] = 0; push_heap_path (heap, tp, calc_index (x0, y0)); xs = md->xs - 1; // あらかじめ1減算しておく ys = md->ys - 1; for (;;) { int e = 0, f = 0, dist, cost, dc[4] = {0, 0, 0, 0}; if (heap[0] == 0) return false; rp = pop_heap_path (heap, tp); x = tp[rp].x; y = tp[rp].y; dist = tp[rp].dist + 10; cost = tp[rp].cost; if (x == x1 && y == y1) break; // dc[0] : y++ の時のコスト増分 // dc[1] : x-- の時のコスト増分 // dc[2] : y-- の時のコスト増分 // dc[3] : x++ の時のコスト増分 if (y < ys && !map_getcellp (md, x , y + 1, cell)) { f |= 1; dc[0] = (y >= y1 ? 20 : 0); e += add_path (heap, tp, x , y + 1, dist, rp, cost + dc[0]); // (x, y+1) } if (x > 0 && !map_getcellp (md, x - 1, y , cell)) { f |= 2; dc[1] = (x <= x1 ? 20 : 0); e += add_path (heap, tp, x - 1, y , dist, rp, cost + dc[1]); // (x-1, y ) } if (y > 0 && !map_getcellp (md, x , y - 1, cell)) { f |= 4; dc[2] = (y <= y1 ? 20 : 0); e += add_path (heap, tp, x , y - 1, dist, rp, cost + dc[2]); // (x , y-1) } if (x < xs && !map_getcellp (md, x + 1, y , cell)) { f |= 8; dc[3] = (x >= x1 ? 20 : 0); e += add_path (heap, tp, x + 1, y , dist, rp, cost + dc[3]); // (x+1, y ) } if ( (f & (2 + 1)) == (2 + 1) && !map_getcellp (md, x - 1, y + 1, cell)) e += add_path (heap, tp, x - 1, y + 1, dist + 4, rp, cost + dc[1] + dc[0] - 6); // (x-1, y+1) if ( (f & (2 + 4)) == (2 + 4) && !map_getcellp (md, x - 1, y - 1, cell)) e += add_path (heap, tp, x - 1, y - 1, dist + 4, rp, cost + dc[1] + dc[2] - 6); // (x-1, y-1) if ( (f & (8 + 4)) == (8 + 4) && !map_getcellp (md, x + 1, y - 1, cell)) e += add_path (heap, tp, x + 1, y - 1, dist + 4, rp, cost + dc[3] + dc[2] - 6); // (x+1, y-1) if ( (f & (8 + 1)) == (8 + 1) && !map_getcellp (md, x + 1, y + 1, cell)) e += add_path (heap, tp, x + 1, y + 1, dist + 4, rp, cost + dc[3] + dc[0] - 6); // (x+1, y+1) tp[rp].flag = 1; if (e || heap[0] >= MAX_HEAP - 5) return false; } if (! (x == x1 && y == y1)) // will never happen... return false; for (len = 0, i = rp; len < 100 && i != calc_index (x0, y0); i = tp[i].before, len++); if (len == 100 || len >= sizeof (wpd->path)) return false; wpd->path_len = len; wpd->path_pos = 0; for (i = rp, j = len - 1; j >= 0; i = tp[i].before, j--) { int dx = tp[i].x - tp[tp[i].before].x; int dy = tp[i].y - tp[tp[i].before].y; int dir; if (dx == 0) { dir = (dy > 0 ? 0 : 4); } else if (dx > 0) { dir = (dy == 0 ? 6 : (dy < 0 ? 5 : 7)); } else { dir = (dy == 0 ? 2 : (dy > 0 ? 1 : 3)); } wpd->path[j] = dir; } return true; }
/*========================================== * path探索 (x0,y0)->(x1,y1) *------------------------------------------ */ int path_search(struct walkpath_data *wpd, map_local *m, int x0, int y0, int x1, int y1, int flag) { int heap[MAX_HEAP + 1]; int i, rp, x, y; int dx, dy; nullpo_ret(wpd); assert (m->gat); map_local *md = m; if (x1 < 0 || x1 >= md->xs || y1 < 0 || y1 >= md->ys || bool(read_gatp(md, x1, y1) & MapCell::UNWALKABLE)) return -1; // easy dx = (x1 - x0 < 0) ? -1 : 1; dy = (y1 - y0 < 0) ? -1 : 1; for (x = x0, y = y0, i = 0; x != x1 || y != y1;) { if (i >= sizeof(wpd->path)) return -1; if (x != x1 && y != y1) { if (!can_move(md, x, y, x + dx, y + dy)) break; x += dx; y += dy; wpd->path[i++] = (dx < 0) ? ((dy > 0) ? DIR::SW : DIR::NW) : ((dy < 0) ? DIR::NE : DIR::SE); } else if (x != x1) { if (!can_move(md, x, y, x + dx, y)) break; x += dx; wpd->path[i++] = (dx < 0) ? DIR::W : DIR::E; } else { // y!=y1 if (!can_move(md, x, y, x, y + dy)) break; y += dy; wpd->path[i++] = (dy > 0) ? DIR::S : DIR::N; } if (x == x1 && y == y1) { wpd->path_len = i; wpd->path_pos = 0; wpd->path_half = 0; return 0; } } if (flag & 1) return -1; struct tmp_path tp[MAX_WALKPATH * MAX_WALKPATH] {}; i = calc_index(x0, y0); tp[i].x = x0; tp[i].y = y0; tp[i].dist = 0; tp[i].dir = DIR::S; tp[i].before = 0; tp[i].cost = calc_cost(&tp[i], x1, y1); tp[i].flag = 0; heap[0] = 0; push_heap_path(heap, tp, calc_index(x0, y0)); while (1) { int e = 0; if (heap[0] == 0) return -1; rp = pop_heap_path(heap, tp); x = tp[rp].x; y = tp[rp].y; if (x == x1 && y == y1) { int len, j; for (len = 0, i = rp; len < 100 && i != calc_index(x0, y0); i = tp[i].before, len++); if (len == 100 || len >= sizeof(wpd->path)) return -1; wpd->path_len = len; wpd->path_pos = 0; wpd->path_half = 0; for (i = rp, j = len - 1; j >= 0; i = tp[i].before, j--) wpd->path[j] = tp[i].dir; return 0; } if (can_move(md, x, y, x + 1, y - 1)) e += add_path(heap, tp, x + 1, y - 1, tp[rp].dist + 14, DIR::NE, rp, x1, y1); if (can_move(md, x, y, x + 1, y)) e += add_path(heap, tp, x + 1, y, tp[rp].dist + 10, DIR::E, rp, x1, y1); if (can_move(md, x, y, x + 1, y + 1)) e += add_path(heap, tp, x + 1, y + 1, tp[rp].dist + 14, DIR::SE, rp, x1, y1); if (can_move(md, x, y, x, y + 1)) e += add_path(heap, tp, x, y + 1, tp[rp].dist + 10, DIR::S, rp, x1, y1); if (can_move(md, x, y, x - 1, y + 1)) e += add_path(heap, tp, x - 1, y + 1, tp[rp].dist + 14, DIR::SW, rp, x1, y1); if (can_move(md, x, y, x - 1, y)) e += add_path(heap, tp, x - 1, y, tp[rp].dist + 10, DIR::W, rp, x1, y1); if (can_move(md, x, y, x - 1, y - 1)) e += add_path(heap, tp, x - 1, y - 1, tp[rp].dist + 14, DIR::NW, rp, x1, y1); if (can_move(md, x, y, x, y - 1)) e += add_path(heap, tp, x, y - 1, tp[rp].dist + 10, DIR::N, rp, x1, y1); tp[rp].flag = 1; if (e || heap[0] >= MAX_HEAP - 5) return -1; } }
/*========================================== * path search (x0,y0)->(x1,y1) * wpd: path info will be written here * flag: &1 = easy path search only * cell: type of obstruction to check for *------------------------------------------*/ bool path_search(struct walkpath_data *wpd, int16 m, int16 x0, int16 y0, int16 x1, int16 y1, int flag, cell_chk cell) { register int i, j, x, y, dx, dy; struct map_data *md; struct walkpath_data s_wpd; if (wpd == NULL) wpd = &s_wpd; // use dummy output variable if (!map[m].cell) return false; md = &map[m]; #ifdef CELL_NOSTACK //Do not check starting cell as that would get you stuck. if (x0 < 0 || x0 >= md->xs || y0 < 0 || y0 >= md->ys) #else if (x0 < 0 || x0 >= md->xs || y0 < 0 || y0 >= md->ys /*|| md->getcellp(md,x0,y0,cell)*/) #endif return false; // Check destination cell if (x1 < 0 || x1 >= md->xs || y1 < 0 || y1 >= md->ys || md->getcellp(md,x1,y1,cell)) return false; if (flag&1) { // Try finding direct path to target // Direct path goes diagonally first, then in straight line. // calculate (sgn(x1-x0), sgn(y1-y0)) dx = ((dx = x1-x0)) ? ((dx<0) ? -1 : 1) : 0; dy = ((dy = y1-y0)) ? ((dy<0) ? -1 : 1) : 0; x = x0; // Current position = starting cell y = y0; i = 0; while( i < ARRAYLENGTH(wpd->path) ) { wpd->path[i] = walk_choices[-dy + 1][dx + 1]; i++; x += dx; // Advance current position y += dy; if( x == x1 ) dx = 0; // destination x reached, no longer move along x-axis if( y == y1 ) dy = 0; // destination y reached, no longer move along y-axis if( dx == 0 && dy == 0 ) break; // success if( md->getcellp(md,x,y,cell) ) break; // obstacle = failure } if( x == x1 && y == y1 ) { // easy path successful. wpd->path_len = i; wpd->path_pos = 0; return true; } return false; // easy path unsuccessful } else { // !(flag&1) // A* (A-star) pathfinding // We always use A* for finding walkpaths because it is what game client uses. // Easy pathfinding cuts corners of non-walkable cells, but client always walks around it. BHEAP_STRUCT_VAR(node_heap, open_set); // 'Open' set // FIXME: This array is too small to ensure all paths shorter than MAX_WALKPATH // can be found without node collision: calc_index(node1) = calc_index(node2). // Figure out more proper size or another way to keep track of known nodes. struct path_node tp[MAX_WALKPATH * MAX_WALKPATH]; struct path_node *current, *it; int xs = md->xs - 1; int ys = md->ys - 1; int len = 0; memset(tp, 0, sizeof(tp)); // Start node i = calc_index(x0, y0); tp[i].parent = NULL; tp[i].x = x0; tp[i].y = y0; tp[i].g_cost = 0; tp[i].f_cost = heuristic(x0, y0, x1, y1); tp[i].flag = SET_OPEN; heap_push_node(&open_set, &tp[i]); // Put start node to 'open' set for(;;) { int e = 0; // error flag // Saves allowed directions for the current cell. Diagonal directions // are only allowed if both directions around it are allowed. This is // to prevent cutting corner of nearby wall. // For example, you can only go NW from the current cell, if you can // go N *and* you can go W. Otherwise you need to walk around the // (corner of the) non-walkable cell. int allowed_dirs = 0; int g_cost; if (BHEAP_LENGTH(open_set) == 0) { BHEAP_CLEAR(open_set); return false; } current = BHEAP_PEEK(open_set); // Look for the lowest f_cost node in the 'open' set BHEAP_POP(open_set, NODE_MINTOPCMP, swap_ptr); // Remove it from 'open' set x = current->x; y = current->y; g_cost = current->g_cost; current->flag = SET_CLOSED; // Add current node to 'closed' set if (x == x1 && y == y1) { BHEAP_CLEAR(open_set); break; } if (y < ys && !md->getcellp(md, x, y+1, cell)) allowed_dirs |= DIR_NORTH; if (y > 0 && !md->getcellp(md, x, y-1, cell)) allowed_dirs |= DIR_SOUTH; if (x < xs && !md->getcellp(md, x+1, y, cell)) allowed_dirs |= DIR_EAST; if (x > 0 && !md->getcellp(md, x-1, y, cell)) allowed_dirs |= DIR_WEST; #define chk_dir(d) ((allowed_dirs & (d)) == (d)) // Process neighbors of current node // TODO: Processing order affects chosen path if there is more than one path with same cost. // In few cases path found by server will be different than path found by game client. if (chk_dir(DIR_SOUTH)) e += add_path(&open_set, tp, x, y-1, g_cost + MOVE_COST, current, heuristic(x, y-1, x1, y1)); // (x, y-1) 4 if (chk_dir(DIR_SOUTH|DIR_WEST) && !md->getcellp(md, x-1, y-1, cell)) e += add_path(&open_set, tp, x-1, y-1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x-1, y-1, x1, y1)); // (x-1, y-1) 3 if (chk_dir(DIR_WEST)) e += add_path(&open_set, tp, x-1, y, g_cost + MOVE_COST, current, heuristic(x-1, y, x1, y1)); // (x-1, y) 2 if (chk_dir(DIR_NORTH|DIR_WEST) && !md->getcellp(md, x-1, y+1, cell)) e += add_path(&open_set, tp, x-1, y+1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x-1, y+1, x1, y1)); // (x-1, y+1) 1 if (chk_dir(DIR_NORTH)) e += add_path(&open_set, tp, x, y+1, g_cost + MOVE_COST, current, heuristic(x, y+1, x1, y1)); // (x, y+1) 0 if (chk_dir(DIR_NORTH|DIR_EAST) && !md->getcellp(md, x+1, y+1, cell)) e += add_path(&open_set, tp, x+1, y+1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x+1, y+1, x1, y1)); // (x+1, y+1) 7 if (chk_dir(DIR_EAST)) e += add_path(&open_set, tp, x+1, y, g_cost + MOVE_COST, current, heuristic(x+1, y, x1, y1)); // (x+1, y) 6 if (chk_dir(DIR_SOUTH|DIR_EAST) && !md->getcellp(md, x+1, y-1, cell)) e += add_path(&open_set, tp, x+1, y-1, g_cost + MOVE_DIAGONAL_COST, current, heuristic(x+1, y-1, x1, y1)); // (x+1, y-1) 5 #undef chk_dir if (e) { BHEAP_CLEAR(open_set); return false; } } for (it = current; it->parent != NULL; it = it->parent, len++); if (len > sizeof(wpd->path)) { return false; } // Recreate path wpd->path_len = len; wpd->path_pos = 0; for (it = current, j = len-1; j >= 0; it = it->parent, j--) { dx = it->x - it->parent->x; dy = it->y - it->parent->y; wpd->path[j] = walk_choices[-dy + 1][dx + 1]; } return true; } // A* end return false; }
/*========================================== * path探索 (x0,y0)->(x1,y1) *------------------------------------------ */ int path_search(struct walkpath_data *wpd,int m,int x0,int y0,int x1,int y1,int flag) { int heap[MAX_HEAP+1]; struct tmp_path tp[MAX_WALKPATH*MAX_WALKPATH]; int i,rp,x,y; struct map_data *md; int dx,dy; if(!map[m].gat) return -1; md=&map[m]; if(x1<0 || x1>=md->xs || y1<0 || y1>=md->ys || (i=read_gatp(md,x1,y1))==1 || i==5) return -1; // easy dx = (x1-x0<0) ? -1 : 1; dy = (y1-y0<0) ? -1 : 1; for(x=x0,y=y0,i=0;x!=x1 || y!=y1;){ if(i>=sizeof(wpd->path)) return -1; if(x!=x1 && y!=y1){ if(!can_move(md,x,y,x+dx,y+dy,flag)) break; x+=dx; y+=dy; wpd->path[i++]=(dx<0) ? ((dy>0)? 1 : 3) : ((dy<0)? 5 : 7); } else if(x!=x1){ if(!can_move(md,x,y,x+dx,y ,flag)) break; x+=dx; wpd->path[i++]=(dx<0) ? 2 : 6; } else { // y!=y1 if(!can_move(md,x,y,x ,y+dy,flag)) break; y+=dy; wpd->path[i++]=(dy>0) ? 0 : 4; } if(x==x1 && y==y1){ wpd->path_len=i; wpd->path_pos=0; wpd->path_half=0; return 0; } } if(flag&1) return -1; memset(tp,0,sizeof(tp)); i=calc_index(x0,y0); tp[i].x=x0; tp[i].y=y0; tp[i].dist=0; tp[i].dir=0; tp[i].before=0; tp[i].cost=calc_cost(&tp[i],x1,y1); tp[i].flag=0; heap[0]=0; push_heap_path(heap,tp,calc_index(x0,y0)); while(1){ int e=0,fromdir; if(heap[0]==0) return -1; rp=pop_heap_path(heap,tp); x=tp[rp].x; y=tp[rp].y; if(x==x1 && y==y1){ int len,j; for(len=0,i=rp;len<100 && i!=calc_index(x0,y0);i=tp[i].before,len++); if(len==100 || len>=sizeof(wpd->path)) return -1; wpd->path_len=len; wpd->path_pos=0; wpd->path_half=0; for(i=rp,j=len-1;j>=0;i=tp[i].before,j--) wpd->path[j]=tp[i].dir; return 0; } fromdir=tp[rp].dir; if(can_move(md,x,y,x+1,y-1,flag)) e+=add_path(heap,tp,x+1,y-1,tp[rp].dist+14,5,rp,x1,y1); if(can_move(md,x,y,x+1,y ,flag)) e+=add_path(heap,tp,x+1,y ,tp[rp].dist+10,6,rp,x1,y1); if(can_move(md,x,y,x+1,y+1,flag)) e+=add_path(heap,tp,x+1,y+1,tp[rp].dist+14,7,rp,x1,y1); if(can_move(md,x,y,x ,y+1,flag)) e+=add_path(heap,tp,x ,y+1,tp[rp].dist+10,0,rp,x1,y1); if(can_move(md,x,y,x-1,y+1,flag)) e+=add_path(heap,tp,x-1,y+1,tp[rp].dist+14,1,rp,x1,y1); if(can_move(md,x,y,x-1,y ,flag)) e+=add_path(heap,tp,x-1,y ,tp[rp].dist+10,2,rp,x1,y1); if(can_move(md,x,y,x-1,y-1,flag)) e+=add_path(heap,tp,x-1,y-1,tp[rp].dist+14,3,rp,x1,y1); if(can_move(md,x,y,x ,y-1,flag)) e+=add_path(heap,tp,x ,y-1,tp[rp].dist+10,4,rp,x1,y1); tp[rp].flag=1; if(e || heap[0]>=MAX_HEAP-5) return -1; } return -1; }
/*========================================== * path探索 (x0,y0)->(x1,y1) *------------------------------------------ */ int path_search_real(struct walkpath_data *wpd,int m,int x0,int y0,int x1,int y1,int easy,cell_t flag) { int x, y, i = 0; int dx, dy; struct map_data *md = &map[m]; if(!map[m].gat) return -1; // path_search2() の場合map_getcellp() の返り値は常に0 if(x0 < 0 || x0 >= md->xs || y0 < 0 || y0 >= md->ys || map_getcellp(md,x0,y0,flag)) return -1; if(x1 < 0 || x1 >= md->xs || y1 < 0 || y1 >= md->ys || map_getcellp(md,x1,y1,flag)) return -1; // easy // この内部では、0 <= x+dx < sx, 0 <= y+dy < sy は保証されている dx = (x1 - x0 < 0) ? -1 : 1; dy = (y1 - y0 < 0) ? -1 : 1; x = x0; y = y0; while(x != x1 || y != y1) { if(i >= MAX_WALKPATH) return -1; if(x != x1 && y != y1) { if(map_getcellp(md,x+dx,y ,flag)) break; if(map_getcellp(md,x ,y+dy,flag)) break; if(map_getcellp(md,x+dx,y+dy,flag)) break; x += dx; y += dy; if(wpd) wpd->path[i++] = walk_choice[-dy+1][dx+1]; } else if(x != x1) { if(map_getcellp(md,x+dx,y,flag)) break; x += dx; if(wpd) wpd->path[i++] = walk_choice[1][dx+1]; } else { // y!=y1 if(map_getcellp(md,x,y+dy,flag)) break; y += dy; if(wpd) wpd->path[i++] = walk_choice[-dy+1][1]; } if(x == x1 && y == y1) { if(wpd) { wpd->path_len = i; wpd->path_pos = 0; } return 0; } } if(!easy) { int xs, ys, rp; int len, j; int heap[MAX_HEAP+1]; struct tmp_path tp[MAX_WALKPATH * MAX_WALKPATH]; memset(tp, 0, sizeof(tp)); i = calc_index(x0,y0); tp[i].x = x0; tp[i].y = y0; tp[i].dist = 0; tp[i].before = 0; tp[i].cost = calc_cost(&tp[i],x1,y1); tp[i].flag = 0; heap[0] = 0; push_heap_path(heap,tp,calc_index(x0,y0)); // あらかじめ1減算しておく xs = md->xs - 1; ys = md->ys - 1; while(1) { int e = 0, f = 0; int dist, cost; int dc[4] = { 0, 0, 0, 0 }; if(heap[0] == 0) return -1; rp = pop_heap_path(heap,tp); x = tp[rp].x; y = tp[rp].y; if(x == x1 && y == y1) break; dist = tp[rp].dist + 10; cost = tp[rp].cost; // dc[0] : y++ の時のコスト増分 // dc[1] : x-- の時のコスト増分 // dc[2] : y-- の時のコスト増分 // dc[3] : x++ の時のコスト増分 if(y < ys && !map_getcellp(md,x ,y+1,flag)) { f |= 1; dc[0] = (y >= y1 ? 20 : 0); e += add_path(heap,tp,x ,y+1,dist,rp,cost+dc[0]); // (x, y+1) } if(x > 0 && !map_getcellp(md,x-1,y ,flag)) { f |= 2; dc[1] = (x <= x1 ? 20 : 0); e += add_path(heap,tp,x-1,y ,dist,rp,cost+dc[1]); // (x-1, y ) } if(y > 0 && !map_getcellp(md,x ,y-1,flag)) { f |= 4; dc[2] = (y <= y1 ? 20 : 0); e += add_path(heap,tp,x ,y-1,dist,rp,cost+dc[2]); // (x , y-1) } if(x < xs && !map_getcellp(md,x+1,y ,flag)) { f |= 8; dc[3] = (x >= x1 ? 20 : 0); e += add_path(heap,tp,x+1,y ,dist,rp,cost+dc[3]); // (x+1, y ) } if( (f & (2+1)) == (2+1) && !map_getcellp(md,x-1,y+1,flag)) e += add_path(heap,tp,x-1,y+1,dist+4,rp,cost+dc[1]+dc[0]-6); // (x-1, y+1) if( (f & (2+4)) == (2+4) && !map_getcellp(md,x-1,y-1,flag)) e += add_path(heap,tp,x-1,y-1,dist+4,rp,cost+dc[1]+dc[2]-6); // (x-1, y-1) if( (f & (8+4)) == (8+4) && !map_getcellp(md,x+1,y-1,flag)) e += add_path(heap,tp,x+1,y-1,dist+4,rp,cost+dc[3]+dc[2]-6); // (x+1, y-1) if( (f & (8+1)) == (8+1) && !map_getcellp(md,x+1,y+1,flag)) e += add_path(heap,tp,x+1,y+1,dist+4,rp,cost+dc[3]+dc[0]-6); // (x+1, y+1) tp[rp].flag = 1; if(e || heap[0] >= MAX_HEAP - 5) return -1; } for(len = 0, i = rp; len < MAX_WALKPATH && i != calc_index(x0,y0); i = tp[i].before, len++); if(len >= MAX_WALKPATH) return -1; if(wpd == NULL) return 0; wpd->path_len = len; wpd->path_pos = 0; for(i = rp, j = len-1; j >= 0; i = tp[i].before, j--) { int tx = tp[i].x - tp[tp[i].before].x; int ty = tp[i].y - tp[tp[i].before].y; wpd->path[j] = walk_choice[-ty+1][tx+1]; } #if 0 // test { x = x0; y = y0; for(i = 0; i < wpd->path_len; i++) { x += dirx[ wpd->path[i] ]; y += diry[ wpd->path[i] ]; if( map_getcellp(md,x,y,flag) ) { printf("path_search_real: cannot move(%d, %d)\n", x, y); return -1; } } if( x != x1 || y != y1 ) { printf("path_search_real: dest position is wrong. ok:(%d, %d) ng:(%d,%d)\n", x1, y1, x, y); return -1; } } #endif return 0; } return -1; }
ERL_NIF_TERM pathfind(ErlNifEnv *env, int argc, ERL_NIF_TERM argv[]) { /*debug("----------------------------\n");*/ int id, x0, y0, x1, y1; ERL_NIF_TERM eid, from, to, head, tail; eid = argv[0]; from = argv[1]; to = argv[2]; // Get the map ID enif_get_int(env, eid, &id); // Get the From X, Y enif_get_list_cell(env, from, &head, &tail); enif_get_int(env, head, &x0); enif_get_int(env, tail, &y0); // Get the To X, Y enif_get_list_cell(env, to, &head, &tail); enif_get_int(env, head, &x1); enif_get_int(env, tail, &y1); struct map_data map = maps[id]; struct tmp_path tp[MAX_WALKPATH * MAX_WALKPATH]; int heap[151]; int rp, xs, ys; ERL_NIF_TERM *path; ERL_NIF_TERM *step; register int i = 0, j = 0, len, x = x0, y = y0, dx = ((dx = x1 - x0) ? ((dx < 0) ? -1 : 1) : 0), dy = ((dy = y1 - y0) ? ((dy < 0) ? -1 : 1) : 0); /* printmap(map); */ /* debug("Pathfinding.\n"); */ /* debug("\tMap: %d (%d x %d)\n", id, map.width, map.height); */ /* debug("\tFrom: (%d, %d)\n", x0, y0); */ /* debug("\tTo: (%d, %d)\n", x1, y1); */ /* debug("\tFirst step: %d\n", at(map, x + dx, y + dy)); */ step = (ERL_NIF_TERM *)malloc(3 * sizeof(ERL_NIF_TERM)); path = (ERL_NIF_TERM *)malloc(MAX_WALKPATH * sizeof(ERL_NIF_TERM)); for (i = 0; i < MAX_WALKPATH && (x != x1 || y != y1) && at(map, x + dx, y + dy) == 0; i++) { x += dx; y += dy; /* debug("OK: (%d, %d)\n", x, y); */ step[0] = enif_make_int(env, x); step[1] = enif_make_int(env, y); step[2] = enif_make_int(env, walk_choices[-dy + 1][dx + 1]); path[i] = enif_make_tuple(env, 3, step[0], step[1], step[2]); if (x == x1) dx = 0; if (y == y1) dy = 0; /* debug("Next cell? %d (Done: %d)\n", at(map, x + dx, y + dy), dx == 0 && dy == 0); */ } // Simple pathfinding was successful if (x == x1 && y == y1) return finish(env, path, step, i); memset(tp, 0, sizeof(tp)); i = calc_index(x0,y0); tp[i].x = x0; tp[i].y = y0; tp[i].dist = 0; tp[i].before = 0; tp[i].cost = calc_cost(&tp[i], x1, y1); tp[i].flag = 0; heap[0] = 0; push_heap_path(heap, tp, calc_index(x0,y0)); xs = map.width - 1; // あらかじめ1減算しておく ys = map.height - 1; while (1) { int e = 0, f = 0, dist, cost, dc[4] = {0, 0, 0, 0}; if(heap[0] == 0) return finish(env, path, step, 0); rp = pop_heap_path(heap,tp); x = tp[rp].x; y = tp[rp].y; dist = tp[rp].dist + 10; cost = tp[rp].cost; if (x == x1 && y == y1) break; // dc[0] : y++ の時のコスト増分 // dc[1] : x-- の時のコスト増分 // dc[2] : y-- の時のコスト増分 // dc[3] : x++ の時のコスト増分 if (y < ys && !at(map, x, y + 1)) { f |= 1; dc[0] = (y >= y1 ? 20 : 0); e += add_path(heap, tp, x, y + 1, dist, rp, cost + dc[0]); // (x, y+1) } if (x > 0 && !at(map, x - 1, y)) { f |= 2; dc[1] = (x <= x1 ? 20 : 0); e += add_path(heap, tp, x - 1, y, dist, rp, cost + dc[1]); // (x-1, y ) } if (y > 0 && !at(map, x, y - 1)) { f |= 4; dc[2] = (y <= y1 ? 20 : 0); e += add_path(heap, tp, x, y - 1, dist, rp, cost + dc[2]); // (x , y-1) } if (x < xs && !at(map, x + 1, y)) { f |= 8; dc[3] = (x >= x1 ? 20 : 0); e += add_path(heap, tp, x + 1, y, dist, rp, cost + dc[3]); // (x+1, y ) } if((f & (2+1)) == (2+1) && !at(map, x - 1, y + 1)) e += add_path(heap, tp, x - 1, y + 1, dist + 4, rp, cost + dc[1] + dc[0] - 6); // (x-1, y+1) if((f & (2+4)) == (2+4) && !at(map, x - 1, y - 1)) e += add_path(heap, tp, x - 1, y - 1, dist + 4, rp, cost + dc[1] + dc[2] - 6); // (x-1, y-1) if((f & (8+4)) == (8+4) && !at(map, x + 1, y - 1)) e += add_path(heap, tp, x + 1, y - 1, dist + 4, rp, cost + dc[3] + dc[2] - 6); // (x+1, y-1) if((f & (8+1)) == (8+1) && !at(map, x + 1, y + 1)) e += add_path(heap, tp, x + 1, y + 1, dist + 4, rp, cost + dc[3] + dc[0] - 6); // (x+1, y+1) tp[rp].flag = 1; if (e || heap[0] >= 150 - 5) return finish(env, path, step, 0); } for (len = 0, i = rp; len < 100 && i != calc_index(x0, y0); i = tp[i].before, len++); if (len == 100 || len >= MAX_WALKPATH) return finish(env, path, step, 0); for (i = rp, j = len - 1; j >= 0; i = tp[i].before, j--) { int dx = tp[i].x - tp[tp[i].before].x; int dy = tp[i].y - tp[tp[i].before].y; /* int dir; */ step[0] = enif_make_int(env, tp[i].x); step[1] = enif_make_int(env, tp[i].y); /* if (dx == 0) */ /* dir = (dy > 0 ? 0 : 4); */ /* else if (dx > 0) */ /* dir = (dy == 0 ? 6 : (dy < 0 ? 5 : 7)); */ /* else */ /* dir = (dy == 0 ? 2 : (dy > 0 ? 1 : 3)); */ step[2] = enif_make_int(env, walk_choices[-dy + 1][dx + 1]); path[j] = enif_make_tuple(env, 3, step[0], step[1], step[2]); } return finish(env, path, step, len); }