1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
|
#include <iostream>
#include <queue>
#include <vector>
#include <map>
#include <algorithm>
#include <cstring>
#include <iomanip>
#include <tuple>
#include <set>
using namespace std;
const int mWidth = 20;
const int mHeight = 20;
const int obstacle = -1;
const int visisted = 1;
int bitmap[mWidth][mHeight];
typedef pair< int, int > Point;
int mDistance( const Point& a, const Point& b ){
return (std::abs( a.first - b.first ) + std::abs( a.second - b.second ));
}
void printMap(){
for( int y = 0; y < mHeight; ++ y ){
for( int x = 0; x < mWidth; ++ x ){
cout << setw(3) << bitmap[x][y];
}
cout << endl;
}
}
// score, step, point
typedef tuple< int, int, Point > Node;
void printNode( const Node& c ){
cout <<"S:"<< get<0>(c) << " ST:" << get<1>(c) << " X:" << get<2>(c).first << " Y:" << get<2>(c).second << endl;
}
int Astar( Point start, Point end ){
priority_queue< Node, vector<Node>, greater<Node> > Q;
vector< Node > debugArr;
map< Point, Point > Graph;
auto push = [&]( int x, int y, int step ){
if( x >= mWidth || x < 0 || y >= mHeight || y < 0 ) return;
if( bitmap[x][y] == obstacle || bitmap[x][y] == visisted ) return;
// keep from re visit
bitmap[x][y] = visisted;
// add to open list
Point pt( x, y );
Q.push( Node( step + mDistance( pt, end ) * 3/2, step, pt ) );
};
push( start.first, start.second, 0 );
if( bitmap[ end.first ][ end.second ] == obstacle ) return -1;
// count the number of blocks checked
int vcount = 0; // DEBUG FLAG
while( !Q.empty() ){
++ vcount; // DEBUG FLAG
const Node current = Q.top();
const Point cpoint = std::get<2>( current );
const int cscore = std::get<0>( current );
const int cstep = std::get<1>( current );
if( cpoint == end ){
cout << "VC " << vcount << endl;
return cstep;
}
push( cpoint.first + 1, cpoint.second, cstep + 1 );
push( cpoint.first - 1, cpoint.second, cstep + 1 );
push( cpoint.first, cpoint.second + 1, cstep + 1 );
push( cpoint.first, cpoint.second - 1, cstep + 1 );
Q.pop();
}
return -1;
}
int main()
{
memset( bitmap, 0, sizeof( bitmap ) );
for( int i = 0; i < 10; ++ i ){
bitmap[3][i] = obstacle;
}
for( int i = 3; i < 10; ++ i ) bitmap[i][10] = obstacle;
for( int i = 11; i < 19; ++ i ) bitmap[10][i] = obstacle;
cout << Astar( Point(0,0), Point( 19, 19 ) ) << " step" << endl;
printMap();
return 0;
}
|
