/// 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; // あらかじめ１減算しておく
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));
// あらかじめ１減算しておく
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; // あらかじめ１減算しておく
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);
}
