void dijkstra(node& p)
{
if ( !ok(p) || stop || isvisited(p) )
return;
// neighbors
node a(p.x+1, p.y); // move right
node b(p.x, p.y+1); // move down
node c(p.x, p.y-1); // move up
node D(p.x-1, p.y); // move left
// tentative distances
uint32_t d = getdist(p);
uint32_t da = sum(d, getcost(a));
uint32_t db = sum(d, getcost(b));
uint32_t dc = sum(d, getcost(c));
uint32_t dd = sum(d, getcost(D));
// update distances
if ( !isvisited(a) && da < getdist(a) ) setdist(a, da);
if ( !isvisited(b) && db < getdist(b) ) setdist(b, db);
if ( !isvisited(c) && dc < getdist(c) ) setdist(c, dc);
if ( !isvisited(D) && dd < getdist(D) ) setdist(D, dd);
// mark this node
setvisited(p);
// has all dest nodes been visited?
if ( allvisited() ) {
// target node is lower right
final = getdist(node(xmax-1, ymax-1));
stop = true;
return;
}
void dijkstra(node& p)
{
if ( !ok(p) || stop || isvisited(p) )
return;
// neighbors
node a(p.x+1, p.y);
node b(p.x, p.y+1);
// tentative distances
uint32_t d = getdist(p);
uint32_t da = sum(d, getcost(a));
uint32_t db = sum(d, getcost(b));
// update distances
if ( !isvisited(a) && da < getdist(a) ) setdist(a, da);
if ( !isvisited(b) && db < getdist(b) ) setdist(b, db);
// mark this node
setvisited(p);
// has dest node been visited?
if ( isvisited(node(xmax-1, ymax-1)) ) {
// ...
uint32_t final = getdist(node(xmax-1, ymax-1));
printf("Finished with distance %u\n", final);
stop = true;
return;
}
int find_closest_on_hm_with_path(struct t_map* map, uint8_t sx, uint8_t sy, uint8_t* heatmap, uint8_t* viewarr, uint8_t* o_x, uint8_t* o_y) {
uint16_t temp_patharr [ HEATMAP_SIZE ];
memset(temp_patharr, 255, sizeof temp_patharr);
temp_patharr [sy * MAP_WIDTH + sx] = 0;
struct pqueue_t* pq = pq_create();
pq_push(pq, (void*)((size_t)sy * MAP_WIDTH + sx + 1),0); // create a queue, set the source area to zero.
//assumes the rest is already filled with 0xFFFF
while (pq_size(pq)) { // while not empty
int yx = (int)(size_t)(pq_pop(pq,NULL)) - 1;
int x = (yx % MAP_WIDTH);
int y = (yx / MAP_WIDTH);
if (heatmap[yx]) { pq_destroy(pq); *o_y = y; *o_x = x; return 0; }
int dir=0; //x and y now contain element with lowest priority
for (dir=0; dir < MD_COUNT; dir++) { // for all neighbors
int nx = x + movediff[dir][0];
int ny = y + movediff[dir][1];
if (!vtile(nx,ny)) continue; //this should be a valid tile!
int cost = getcost(map,NULL,nx,ny,viewarr);
if (cost == -1) continue;
if (dir % 2) {
cost = (cost * 3) / 2;
}
int alt = temp_patharr[y * MAP_WIDTH + x] + cost;
if (alt < temp_patharr[ny * MAP_WIDTH + nx]) {
temp_patharr[ny * MAP_WIDTH + nx] = alt;
pq_push (pq, (void*) (ny * MAP_WIDTH + nx + 1), alt);
}
}
}
pq_destroy(pq);
*o_x = 255;
*o_y = 255;
return 0;
}
//void BiDirAStar::explore(int cur_node, double cur_cost, int dir, std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > &que)
void BiDirAStar::explore(int cur_node, double cur_cost, int dir, MinHeap &que)
{
size_t i;
// Number of connected edges
auto con_edge = m_vecNodeVector[cur_node].Connected_Edges_Index.size();
double edge_cost;
for(i = 0; i < con_edge; i++)
{
auto edge_index = m_vecNodeVector[cur_node].Connected_Edges_Index[i];
// Get the edge from the edge list.
GraphEdgeInfo edge = m_vecEdgeVector[edge_index];
// Get the connected node
int new_node = m_vecNodeVector[cur_node].Connected_Nodes[i];
#if 0 // mult is set but not used
int mult;
if(edge.Direction == 0)
mult = 1;
else
mult = dir;
#endif
if(cur_node == edge.StartNode)
{
// Current node is the startnode of the edge. For forward search it should use forward cost, otherwise it should use the reverse cost,
// i.e. if the reverse direction is valid then this node may be visited from the end node.
if(dir > 0)
edge_cost = edge.Cost;
else
edge_cost = edge.ReverseCost;
// Check if the direction is valid for exploration
if(edge.Direction == 0 || edge_cost >= 0.0)
{
//edge_cost = edge.Cost * mult;
// Check if the current edge gives better result
if(cur_cost + edge_cost < getcost(new_node, dir))
{
// explore the node, and push it in the queue. the value in the queue will also contain the heuristic cost
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost + gethcost(new_node, dir), new_node));
// Update the minimum cost found so far.
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
else
{
// Current node is the endnode of the edge. For forward search it should use reverse cost, otherwise it should use the forward cost,
// i.e. if the forward direction is valid then this node may be visited from the start node.
if(dir > 0)
edge_cost = edge.ReverseCost;
else
edge_cost = edge.Cost;
// Check if the direction is valid for exploration
if(edge.Direction == 0 || edge_cost >= 0.0)
{
//edge_cost = edge.ReverseCost * mult;
// Check if the current edge gives better result
if(cur_cost + edge_cost < getcost(new_node, dir))
{
// explore the node, and push it in the queue. the value in the queue will also contain the heuristic cost
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost + gethcost(new_node, dir), new_node));
// Update the minimum cost found so far.
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
}
}
void BiDirDijkstra::explore(int cur_node, double cur_cost, int dir, std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > &que)
{
int i;
// Number of connected edges
int con_edge = m_vecNodeVector[cur_node]->Connected_Edges_Index.size();
double edge_cost;
for(i = 0; i < con_edge; i++)
{
int edge_index = m_vecNodeVector[cur_node]->Connected_Edges_Index[i];
// Get the edge from the edge list.
GraphEdgeInfo edge = m_vecEdgeVector[edge_index];
// Get the connected node
int new_node = m_vecNodeVector[cur_node]->Connected_Nodes[i];
if(cur_node == edge.StartNode)
{
// Current node is the startnode of the edge. For forward search it should use forward cost, otherwise it should use the reverse cost,
// i.e. if the reverse direction is valid then this node may be visited from the end node.
if(dir > 0)
edge_cost = edge.Cost;
else
edge_cost = edge.ReverseCost;
// Check if the direction is valid for exploration
if(edge.Direction == 0 || edge_cost >= 0.0)
{
// Check if the current edge gives better result
if(cur_cost + edge_cost < getcost(new_node, dir))
{
// explore the node, and push it in the queue
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost, new_node));
// Update the minimum cost found so far.
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
else
{
// Current node is the endnode of the edge. For forward search it should use reverse cost, otherwise it should use the forward cost,
// i.e. if the forward direction is valid then this node may be visited from the start node.
if(dir > 0)
edge_cost = edge.ReverseCost;
else
edge_cost = edge.Cost;
// Check if the direction is valid for exploration
if(edge.Direction == 0 || edge_cost >= 0.0)
{
// Check if the current edge gives better result
if(cur_cost + edge_cost < getcost(new_node, dir))
{
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost, new_node));
// Update the minimum cost found so far.
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
}
}
void BiDirDijkstra::explore(int cur_node, double cur_cost, int dir, std::priority_queue<PDI, std::vector<PDI>, std::greater<PDI> > &que)
{
int i;
int con_edge = m_vecNodeVector[cur_node].Connected_Edges_Index.size();
double edge_cost;
for(i = 0; i < con_edge; i++)
{
int edge_index = m_vecNodeVector[cur_node].Connected_Edges_Index[i];
GraphEdgeInfo edge = m_vecEdgeVector[edge_index];
int new_node = m_vecNodeVector[cur_node].Connected_Nodes[i];
int mult;
if(edge.Direction == 0)
mult = 1;
else
mult = dir;
if(cur_node == edge.StartNode)
{
if(dir > 0)
edge_cost = edge.Cost;
else
edge_cost = edge.ReverseCost;
if(edge.Direction == 0 || edge_cost >= 0.0)
{
//edge_cost = edge.Cost * mult;
if(cur_cost + edge_cost < getcost(new_node, dir))
{
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost, new_node));
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
else
{
if(dir > 0)
edge_cost = edge.ReverseCost;
else
edge_cost = edge.Cost;
if(edge.Direction == 0 || edge_cost >= 0.0)
{
//edge_cost = edge.ReverseCost * mult;
if(cur_cost + edge_cost < getcost(new_node, dir))
{
setcost(new_node, dir, cur_cost + edge_cost);
setparent(new_node, dir, cur_node, edge.EdgeID);
que.push(std::make_pair(cur_cost + edge_cost, new_node));
if(getcost(new_node, dir) + getcost(new_node, dir * -1) < m_MinCost)
{
m_MinCost = getcost(new_node, dir) + getcost(new_node, dir * -1);
m_MidNode = new_node;
}
}
}
}
}
}
