//print the shortest sequence of modifications that goes from s to t.
//reads from the global table, so only works after string_compare is called.
void reconstruct_path(char* s, char* t, int i, int j)
{
switch(m[i][j].parent)
{
case -1 : return;
case MATCH : reconstruct_path(s,t,i-1,j-1);
printf("M");
return;
case INSERT : reconstruct_path(s,t,i,j-1);
printf("I");
return;
case DELETE : reconstruct_path(s,t,i-1,j);
printf("D");
return;
case CHANGE : reconstruct_path(s,t,i-1,j-1);
if (m[i-1][j-1].parent == SWAP)
printf("S");
else
printf("C");
return;
case SWAP : reconstruct_path(s,t,i-1,j-1);
printf("S");
return;
}
}
// print the start and target string, the edit distence, and the edit sequence.
// the space for the first character has to be manually added.
void test(char* str1, char* str2)
{
int res = string_compare(str1, str2);
printf("edit distance from \"%s\" to \"%s\" is %d: ", str1, str2, res);
reconstruct_path(str1, str2, strlen(str1) - 1, strlen(str2) - 1);
printf("\n");
}
std::vector<Location> Enemy1::getPath()
{
std::unordered_map<Location, Location, LocationHash, Equal> came_from;
std::unordered_map<Location, int, LocationHash, Equal> cost_so_far;
path_search(*levelGrid, currentSquare[FIRST_SQUARE_ID], currentGoal, came_from, cost_so_far);
auto path = reconstruct_path(currentSquare[FIRST_SQUARE_ID], currentGoal, came_from);
return path;
}
List *Path(World *world, Point *source, Point *destination)
{
int tentative_gscore;
List *result = NULL;
int tries = 0;
Hashmap *nodes = Hashmap_create(cmp, hash);
Hashmap *closedset = Hashmap_create(cmp, hash);
PQueue *openset = PQueue_create(cmp, hash);
Node *current;
Node *start = Node_create(source, 0, 0, NULL);
Hashmap_set(nodes, start, start);
start->fscore = start->gscore + heuristic_cost_estimate(start->point, destination);
PQueue_push(openset, start, start->fscore);
while(!PQueue_empty(openset) &&
tries < 300) {
tries++;
current = PQueue_pop(openset);
Hashmap_set(closedset, current, current);
if(POINT_EQ(current->point, destination)) {
result = reconstruct_path(current);
break;
} else {
List *neighbours = neighbours_list(world, current->point, destination, nodes);
LIST_FOREACH(neighbours, first, next, cur) {
Node *neighbour = cur->value;
if(Hashmap_get(closedset, neighbour) == NULL) {
tentative_gscore = current->gscore + 1;
if(!PQueue_contains(openset, neighbour) ||
tentative_gscore > neighbour->gscore) {
if(!PQueue_contains(openset, neighbour)) {
neighbour->came_from = current;
neighbour->gscore = tentative_gscore;
neighbour->fscore = neighbour->gscore + heuristic_cost_estimate(neighbour->point, destination);
PQueue_push(openset, neighbour, neighbour->fscore);
}
}
}
}
List_destroy(neighbours);
}
}
reconstruct_path(char *s, char *t, int i, int j)
{
/*printf("trace (%d,%d)\n",i,j);*/
if (m[i][j].parent == -1) return;
if (m[i][j].parent == MATCH) {
reconstruct_path(s,t,i-1,j-1);
match_out(s, t, i, j);
return;
}
if (m[i][j].parent == INSERT) {
reconstruct_path(s,t,i,j-1);
insert_out(t,j);
return;
}
if (m[i][j].parent == DELETE) {
reconstruct_path(s,t,i-1,j);
delete_out(s,i);
return;
}
}
int main(void) {
int i, j;
char s[MAXLEN], t[MAXLEN]; /* input strings */
s[0] = t[0] = ' ';
scanf("%s", &(s[1])); scanf("%s", &(t[1]));
printf("matching cost = %d \n", string_compare(s, t));
print_matrix(s, t, TRUE); printf("\n"); print_matrix(s, t, FALSE);
goal_cell(s, t, &i, &j); printf("%d %d\n", i, j);
reconstruct_path(s,t, i ,j); printf("\n");
return 0;
}
int
astar(start, goal)
{
int k, min, ind, m;
Fscore = g_score(start) + Hcost(start, goal) //Estimated total cost from start to goal.
openset[i] = start;
i++;
m=i;
neighbor[4] = {0,0,0,0};
while(m != -1)
{
min = F_score[m-1]
for(k=m-2; k>=0; k--)
{
if(min > F_score[k])
{
min = F_score[k];
ind = k;
}
}
current = openset[ind];
if(current == goal)
return reconstruct_path(goal);
i--;
closedset[l] = current;
l++;
neighbor[0] =
neighbor[1] =
neighbor[2] =
neighbor[3] =
alpha
for(k=0; k<alpha; k++)
{
tentative_g_score = g_score(current) + dist_between(current, neighbor);
tentative_f_score = tentative_g_score + Hcost(neighbor, goal)
neighbor_f_score = g_score(neighbor[i]) + Hcost(neighbor[i], goal);
if((search(closedset, neighbor[i]) == 1) && (tentative_f_score >= neighbor_f_score))
continue;
if((search(openset,neighbor[i],) == 0) || (tentative_f_score < neighbor_f_score))
{
came_from[] = current;
if(search(openset, neighbor[i]) == 0)
openset[] = neighbor[i];
}
}
}
return 0;
}
vector<cell> PathPlanner::astar()
{
this->fill_heuristic();
this->fill_g_f(_start);
int size = _open_list.size();
while (!_open_list.empty())
{
cell lowest_f_cell = find_lowest_f_score();
if (lowest_f_cell.x_Coordinate == _goal.x_Coordinate &&
(lowest_f_cell.y_Coordinate == _goal.y_Coordinate))
{
reconstruct_path();
return _path;
}
std::set<cell>::iterator it_openlist;
std::set<cell>::iterator it_closelist;
_open_list.erase(lowest_f_cell);
fill_g_f(lowest_f_cell);
}
return _path;
}
main() {
int i,j;
char s[MAXLEN],t[MAXLEN]; /* input strings */
s[0] = t[0] = ' ';
printf("enter p :\n");
scanf("%s",&(s[1]));
printf("enter t :\n");
scanf("%s",&(t[1]));
printf("matching cost = %d \n", string_compare(s,t, strlen(s)-1,strlen(t)-1));
print_matrix(s,t,TRUE);
printf("\n");
print_matrix(s,t,FALSE);
goal_cell(s,t,&i,&j);
printf("%d %d\n",i,j);
reconstruct_path(s,t,i,j);
printf("\n");
}
Route * ww_routing_astar(RoutingIndex *ri, RoutingProfile *profile,
int *from_ids, double to_lat, double to_lon, double tolerance) {
List *closedset, *openset, *l;
AStarScore *sc;
RoutingNode *from, *to;
int from_index, to_index, from_id, i;
Route *result = NULL;
// Set up the lists of open and closed nodes
closedset = NULL;
openset = NULL;
for (i = 0, from_id = from_ids[i]; from_id >= 0; from_id = from_ids[++i]) {
// Find the start nodes in the index
from_index = routing_index_find_node(ri, from_id);
from = &(ri->nodes[from_index]);
if (from_index < 0 || to_index < 0)
continue;
// Set up the score for the start nodes
sc = malloc(sizeof(AStarScore));
sc->n_index = from_index;
sc->node = &(ri->nodes[sc->n_index]);
sc->way = NULL;
sc->came_from = NULL;
sc->g = 0;
sc->h = distance(from->lat, from->lon, to_lat, to_lon);
sc->f = sc->h;
openset = list_prepend(openset, sc);
}
// Loop until there are no more nodes in the open set
while (openset) {
sc = pop_min_f_from_list(&openset);
closedset = list_prepend(closedset, sc);
if (sc->h <= tolerance) {
// Found the optimal route
result = reconstruct_path(sc);
// Finished, goto the end for cleanup
goto end;
}
// Give up is longer than maximum length
if (sc->g > profile->max_route_length)
goto end;
// Check if node is a dead end
if (sc->node->way.end == 0)
continue;
// Check all the nodes connected to this node
int w_index;
for (w_index = sc->node->way.start; w_index < sc->node->way.end; w_index++) {
RoutingWay *w = &(ri->ways[w_index]);
RoutingTagSet *tagset = (void *)ri->tagsets + w->tagset;
AStarScore *sc2 = malloc(sizeof(AStarScore));
sc2->n_index = w->next;
sc2->node = &(ri->nodes[sc2->n_index]);
sc2->way = w;
// Check if node is in closed set, if so skip it
if (list_find(closedset, sc2, score_compare_cb)) {
free(sc2);
continue;
}
// Calculate the tentative new score
double tentative_g_score = sc->g + effective_distance(profile, tagset, sc->node->lat, sc->node->lon,
sc2->node->lat, sc2->node->lon);
// Is it better than the previous best path?
int tentative_is_better = 0;
l = list_find(openset, sc2, score_compare_cb);
if (!l) {
// Node not previously visited
openset = list_prepend(openset, sc2);
tentative_is_better = 1;
} else {
// Node visited, check if new path is better
free(sc2);
sc2 = l->data;
if (tentative_g_score < sc2->g) {
tentative_is_better = 1;
}
}
// If new path is better, update the score
if (tentative_is_better == 1) {
sc2->came_from = sc;
sc2->g = tentative_g_score;
sc2->h = distance(sc2->node->lat, sc2->node->lon, to_lat, to_lon);
sc2->f = sc2->g + sc2->h;
}
}
}
end:
// Clean up
l = openset;
while (l) {
List *ll = l->next;
free(l->data);
free(l);
l = ll;
}
l = closedset;
while (l) {
List *ll = l->next;
free(l->data);
free(l);
l = ll;
}
return result;
}
/*
* pathfinder(info,monster, x1, y1, x2, y2)
*
* Get path between x1,y1 and x2,y2.
* Uses info->is_walkable to determine if monster can
* walk on grid position (x,y). Passes monster to this function
*/
node_t *
pathfinder(pathfinder_info *info, void *monster, int x1, int y1, int x2, int y2)// int *x2_ptr, int *y2_ptr)
{
node_t *start_node;
node_t *node;
node_t *neighbour;
int steps;
unsigned char temp_g_score;
start_node = node_new_pos(info, x1, y1);
bin_heap_clear(info->open_heap);
bin_heap_add(info->open_heap, start_node);
/* Set all scores to 0 */
memset(info->g_score, 0, info->grid_width*info->grid_height);
memset(info->h_score, 0, info->grid_width*info->grid_height);
/* Set all nodetypes to 0 */
memset(info->node_type, 0, info->grid_width*info->grid_height);
#define P(node) node->x + node->y*info->grid_width
info->g_score[P(start_node)] = 0;
info->h_score[P(start_node)] =
manhattan(x1, y1, x2, y2);
/* cost = f_score */
start_node->cost = info->g_score[P(start_node)] + info->h_score[P(start_node)];
steps = 0;
while (info->open_heap->heap_size) {
node = bin_heap_get_min(info->open_heap);
steps ++;
if (node->x == x2 &&
node->y == y2) {
/* Reverse path, and remove path
* from node-list
*/
node = reconstruct_path(info, node);
/* clear up open-heap */
bin_heap_clear(info->open_heap);
pathfinder_free_nodes(info);
return node;
}
bin_heap_del_min(info->open_heap);
/* Set type to CLOSED */
info->node_type[P(node)] = TYPE_CLOSED;
int direction;
/* Look at node's neighbours */
for (direction=0;direction<4;direction++) {
int x,y;
x = node->x + mod_x[direction];
y = node->y + mod_y[direction];
/* Don't walk off map */
if (x < 0 || y < 0 ||
x > info->grid_width - 1 ||
y > info->grid_height - 1)
continue;
/* Don't go back */
if (node->parent &&
node->parent->x == x && node->parent->y == y)
continue;
/* check if we can walk that way */
if (!info->is_walkable(x, y, monster))
continue;
temp_g_score = info->g_score[P(node)] + 1;
char added = 0;
char type;
int neighbour_idx = 0;
int curr_score;
/* If neighbour in OPEN-set and cost is lower */
type = info->node_type[x + y*info->grid_width];
curr_score = info->g_score[x + y*info->grid_width];
if (type != TYPE_CLOSED &&
(curr_score == 0 ||
temp_g_score < curr_score)) {
/* Neighbour not in OPEN and not in CLOSED */
if (type != TYPE_OPEN) {
added = 1;
neighbour = node_new_pos(info, x, y);
}else{
neighbour_idx = find_node(info->open_heap, x, y);
neighbour = info->open_heap->data[neighbour_idx];
}
info->g_score[P(neighbour)] = temp_g_score;
info->h_score[P(neighbour)] = manhattan(x, y, x2, y2);
neighbour->cost =
info->g_score[P(neighbour)] + info->h_score[P(neighbour)];
neighbour->parent = node;
/* We've just changed the cost for this node,
so update the binheap
*/
if (! added)
bin_heap_bubble_up(info->open_heap, neighbour_idx);
else {
bin_heap_add(info->open_heap, neighbour);
info->node_type[P(neighbour)] = TYPE_OPEN;
}
}
}
}
pathfinder_free_nodes(info);
return NULL;
}
struct a_star_path *a_star(void *context, void *start, void *goal,
int maxnodes,
a_star_node_cost_fn distance,
a_star_node_cost_fn cost_estimate,
a_star_neighbor_iterator_fn nth_neighbor)
{
struct nodeset *openset, *closedset;
struct node_map *came_from;
struct score_map *gscore, *fscore;
void *neighbor, *current;
float tentative_gscore;
int i, n;
void **answer = NULL;
int answer_count = 0;
struct a_star_path *return_value;
closedset = nodeset_new(maxnodes);
openset = nodeset_new(maxnodes);
came_from = node_map_new(maxnodes);
gscore = score_map_new(maxnodes);
fscore = score_map_new(maxnodes);
nodeset_add_node(openset, start);
score_map_add_score(gscore, start, 0.0);
score_map_add_score(fscore, start, cost_estimate(context, start, goal));
while (!nodeset_empty(openset)) {
current = lowest_score(openset, fscore);
if (current == goal) {
reconstruct_path(came_from, current, &answer, &answer_count, maxnodes);
break;
}
nodeset_remove_node(openset, current);
nodeset_add_node(closedset, current);
n = 0;
while ((neighbor = nth_neighbor(context, current, n))) {
n++;
if (nodeset_contains_node(closedset, neighbor))
continue;
tentative_gscore = score_map_get_score(gscore, current) + distance(context, current, neighbor);
if (!nodeset_contains_node(openset, neighbor))
nodeset_add_node(openset, neighbor);
else if (tentative_gscore >= score_map_get_score(gscore, neighbor))
continue;
node_map_set_from(came_from, neighbor, current);
score_map_add_score(gscore, neighbor, tentative_gscore);
score_map_add_score(fscore, neighbor,
score_map_get_score(gscore, neighbor) +
cost_estimate(context, neighbor, goal));
}
}
free(closedset);
free(openset);
free(came_from);
free(gscore);
free(fscore);
if (answer_count == 0) {
return_value = NULL;
} else {
return_value = malloc(sizeof(*return_value) + sizeof(return_value->path[0]) * answer_count);
return_value->node_count = answer_count;
for (i = 0; i < answer_count; i++) {
return_value->path[answer_count - i - 1] = answer[i];
}
}
free(answer);
return return_value;
}
std::vector<edge*> world_map::find_path(vertex *start, vertex *end)
{
std::cout << "Beginning pathfinding\n";
std::set<vertex*> closed_set; // fully determined
std::set<vertex*> open_set; // in consideration
open_set.insert(start);
for (size_t i = 0; i < vs.size(); ++i)
{
vs[i]->g_score = LARGE_VALUE;
vs[i]->f_score = LARGE_VALUE;
}
start->g_score = 0;
start->f_score = heuristic_distance(start, end);
while (open_set.size() > 0)
{
vertex *current = *std::min_element(open_set.begin(), open_set.end(), f_lessthan()); // lowest f_scored node
std::cout << "Selected vertex with f score " << current->f_score << "\n";
if (current == end) {
std::cout << "Reached end vertex\n";
return reconstruct_path(start, end);
}
open_set.erase(current);
closed_set.insert(current);
// std::cout << current->edges.size() << " neighbours.\n";
for (size_t i = 0; i < current->edges.size(); ++i)
{
edge *e = current->edges[i];
vertex *neighbour = e->other(current);
//std::cout << "Considering vertex at (" << neighbour->posx << ", " << neighbour->posy << ")\n";
if (closed_set.find(neighbour) != closed_set.end())
{
// if we've already fully determined this node, skip
//std::cout << "Already closed set.\n";
continue;
}
float tentative_g_score = current->g_score + e->length;
if (open_set.find(neighbour) == open_set.end())
{
//std::cout << "Adding to open set\n";
open_set.insert(neighbour);
}
else if (tentative_g_score >= neighbour->g_score)
{
//std::cout << "Already in open set, score not improved upon\n";
// we have already considered this node and found a path as good
// or better
continue;
}
//std::cout << "Best score so far for this node.\n";
// Otherwise this is the best path found so far for this node
neighbour->came_from = e;
neighbour->g_score = tentative_g_score;
neighbour->f_score = tentative_g_score + heuristic_distance(neighbour, end);
}
}
std::cout << "Pathfinding failed!\n";
return std::vector<edge*>(); // Failed!
}
int main() {
init();
std::cout << sizeof(D) << std::endl;
#ifdef WITH_PAPI
int EventSet = PAPI_NULL;
int retval;
char errstring[PAPI_MAX_STR_LEN];
long long values[NUM_EVENTS];
if((retval = PAPI_library_init(PAPI_VER_CURRENT)) != PAPI_VER_CURRENT )
{
fprintf(stderr, "Error: %s\n", errstring);
exit(1);
}
/* Creating event set */
if ((retval=PAPI_create_eventset(&EventSet)) != PAPI_OK)
ERROR_RETURN(retval);
/* Add the array of events PAPI_TOT_INS and PAPI_TOT_CYC to the eventset*/
if ((retval=PAPI_add_event(EventSet, PAPI_L1_DCM)) != PAPI_OK)
ERROR_RETURN(retval);
if ( PAPI_add_event( EventSet, PAPI_L2_DCM ) != PAPI_OK)
printf("Error PAPI_add_event \n" );
/* Start counting */
if ( (retval=PAPI_start(EventSet)) != PAPI_OK)
ERROR_RETURN(retval);
#endif
for(int i = 0; i <= n; i++) {
for(int j = 0; j <= n; j++) {
// Calculate D
if(i > 0 && j > 0) {
D[i][j] = D[i-1][j];
if(G[i-1][j] + GI < D[i][j]) {
D[i][j] = G[i-1][j] + GI;
}
D[i][j] += GE;
}
if(i == 0 && j > 0) {
D[i][j] = G[0][j] + GE;
}
// Calculate I
if(i > 0 && j > 0) {
I[i][j] = I[i][j-1];
if(G[i][j-1] + GI < I[i][j]) {
I[i][j] = G[i][j-1] + GI;
}
I[i][j] += GE;
}
if(i > 0 && j == 0) {
I[i][j] = G[i][0] + GE;
}
// Calculate G
if(i > 0 && j > 0) {
G[i][j] = G[i-1][j-1]+S(X[i-1],Y[j-1]);
if(D[i][j] < G[i][j]) {
G[i][j] = D[i][j];
}
if(I[i][j] < G[i][j]) {
G[i][j] = I[i][j];
}
}
}
}
#ifdef WITH_PAPI
if ( (retval=PAPI_stop(EventSet,values)) != PAPI_OK)
ERROR_RETURN(retval);
std::cout << "L1 Misses: " << values[0] << " L2 Misses: " << values[1] << std::endl;
/*
double tot_access = 4 * n * n *n;
double miss_ratio = values[0] / tot_access;
printf("%d\t%f\n",n,miss_ratio);
*/
if ( (retval=PAPI_remove_event(EventSet,PAPI_L1_DCM))!=PAPI_OK)
ERROR_RETURN(retval);
if ( (retval=PAPI_remove_event(EventSet,PAPI_L2_DCM))!=PAPI_OK)
ERROR_RETURN(retval);
/* Free all memory and data structures, EventSet must be empty. */
if ( (retval=PAPI_destroy_eventset(&EventSet)) != PAPI_OK)
ERROR_RETURN(retval);
/* free the resources used by PAPI */
PAPI_shutdown();
#endif
std::cout << G[n][n] << std::endl;
reconstruct_path();
clean_up();
}
// CLASS FUNCTIONS
std::vector<MapNode*> JPS::get_path
(
Map* map,
MapNode* start,
MapNode* end
)
{
start->set_data<AStarNodeData>(new AStarNodeData(0, heuristic(start,end)));
m_open_set.insert(start);
m_open_queue.push(start);
AStarNodeData* s_data = start->get_data<AStarNodeData>();
std::unordered_map<MapNode*,MapNode*> came_from;
// The return path
std::vector<MapNode*> path;
while (m_open_queue.size() > 0)
{
// Get the first item in the min-heap
MapNode* current = m_open_queue.top();
m_open_set.erase(current);
m_open_queue.pop();
AStarNodeData* c_data = current->get_data<AStarNodeData>();
// Short-circuit for the end
if (!current->is_seen() && c_data->f_score() < s_data->f_score())
{
int dist = path_length(current, end);
if (AStarNodeData* data = end->get_data<AStarNodeData>())
{
data->g_score = data->g_score + dist;
data->h_score = 0;
}
else
{
end->set_data<AStarNodeData>(
new AStarNodeData(c_data->g_score + dist, 0)
);
}
path = reconstruct_path(came_from, current);
break;
}
if (current == end)
{
path = reconstruct_path(came_from, end);
break;
}
m_closed_set.insert(current);
for (MapNode* neighbor : get_successors(map, current, end))
{
bool is_in_open_set = m_open_set.find(neighbor) != m_open_set.end();
bool is_in_closed_set = m_closed_set.find(neighbor) != m_closed_set.end();
// If it's in the closed set skip
if (is_in_closed_set)
{
continue; // Ignore the neighbor which is already evaluated.
}
AStarNodeData* n_data = neighbor->get_data<AStarNodeData>();
// The distance from start to goal passing through current and the neighbor.
int tentative_g_score = c_data->g_score + path_length(current,neighbor);
if (is_in_open_set && tentative_g_score >= n_data->g_score)
{
continue; // This is not a better path.
}
// This path is the best until now. Record it!
came_from[neighbor] = current;
if (n_data)
{
n_data->g_score = tentative_g_score;
n_data->h_score = heuristic(neighbor, end);
}
else
{
neighbor->set_data<AStarNodeData>(
new AStarNodeData(
tentative_g_score,
heuristic(neighbor, end)
)
);
}
if (!is_in_open_set)
{
m_open_set.insert(neighbor);
m_open_queue.push(neighbor);
}
}
}
return path;
}
