1 #include <stdlib.h> 2 #include <stdio.h> 3 #include <string.h> 4 #include <math.h> 5 6 7 #define NODEEQUAL(a, b) (*((unsigned int *)(a)) == *((unsigned int *)(b))) 8 //#define GETDIST(a, b) ((abs((a)->x - (b)->x) + abs((a)->y - (b)->y)) * 10) 9 //#define GETDIST(a, b) ((int)(10 * sqrt((float)((a)->x - (b)->x) * ((a)->x - (b)->x) + ((a)->y - (b)->y) * ((a)->y - (b)->y)))) 10 11 #define WORLDSIZEX 8 12 #define WORLDSIZEY 6 13 #define OBS ((char)1) 14 15 16 17 char g_world[WORLDSIZEY][WORLDSIZEX] = 18 { 19 {0, 1, 0, 0, 0, 0, 0, 0}, 20 {0, 0, 1, 1, 1, 0, 0, 0}, 21 {0, 1, 1, 0, 1, 0, 0, 0}, 22 {0, 0, 0, 0, 1, 1, 1, 0}, 23 {0, 1, 1, 1, 0, 0, 1, 0}, 24 {0, 0, 0, 0, 1, 0, 0, 0} 25 }; 26 27 typedef struct tag_loc 28 { 29 short int x, y; 30 } loc_t, *loc_ptr; 31 32 33 typedef struct tag_node 34 { 35 loc_t loc; 36 int f, g; 37 struct tag_node* father; 38 struct tag_node* next; 39 } node_t, *node_ptr; 40 41 42 loc_t g_start = {0, 0}; 43 loc_t g_end = {7, 4}; 44 loc_t g_aroundpts[8] = 45 { 46 { -1, -1}, { -1, 0}, { -1, 1}, {0, 1}, {1, 1}, {1, 0}, {1, -1}, {0, -1} 47 }; 48 49 50 void print_path( node_ptr lst ) 51 { 52 if ( lst ) 53 { 54 print_path( lst->father ); 55 fprintf( stdout, "(%d,%d) ", lst->loc.x, lst->loc.y ); 56 } 57 } 58 59 void list_destroy( node_ptr lst ) 60 { 61 node_ptr p; 62 63 while ( lst ) 64 { 65 p = lst->next; 66 free( lst ); 67 lst = p; 68 } 69 } 70 71 int get_dist( loc_ptr a, loc_ptr b ) 72 { 73 unsigned int x, y; 74 75 x = abs( a->x - b->x ); 76 y = abs( a->y - b->y ); 77 78 if ( x == 0 ) 79 { 80 return 10 * y; 81 } 82 83 if ( y == 0 ) 84 { 85 return 10 * x; 86 } 87 88 return ( x + y ) * 7; 89 } 90 91 node_ptr create_closed( loc_ptr start, loc_ptr end ) 92 { 93 int h; 94 node_ptr node; 95 96 if ( start->x < 0 || start->x >= WORLDSIZEX || \ 97 start->y < 0 || start->y >= WORLDSIZEY || \ 98 end->x < 0 || end->x >= WORLDSIZEX || \ 99 end->y < 0 || end->y >= WORLDSIZEY ) 100 { 101 fprintf( stderr, "起始或目标坐标非法!\n" ); 102 return NULL; 103 } 104 105 node = ( node_ptr )malloc( sizeof( node_t ) ); 106 if ( node == NULL ) 107 { 108 fprintf( stderr, "malloc 内存不足!\n" ); 109 return NULL; 110 } 111 112 memset( node, 0, sizeof( node_t ) ); 113 memcpy( &node->loc, start, sizeof( loc_t ) ); 114 h = get_dist( &node->loc, end ); 115 //node->g = 0; 116 node->f = node->g + h; 117 //node->father = NULL; 118 //node->next = NULL; 119 120 return node; 121 } 122 123 static void _list_inst( node_ptr* header, node_ptr node ) 124 { 125 node_ptr h, p; 126 127 h = *header; 128 129 if ( h->f >= node->f ) 130 { 131 node->next = h; 132 *header = node; 133 } 134 else 135 { 136 p = h->next; 137 while ( p ) 138 { 139 if ( p->f >= node->f ) 140 { 141 h->next = node; 142 node->next = p; 143 break; 144 } 145 h = p; 146 p = p->next; 147 } 148 149 if ( p == NULL ) 150 { 151 h->next = node; 152 node->next = NULL; 153 } 154 } 155 } 156 157 static int _updt_opened( node_ptr* header, node_ptr node ) 158 { 159 node_ptr h, p, tmp; 160 161 if ( *header == NULL ) 162 { 163 tmp = ( node_ptr )malloc( sizeof( node_t ) ); 164 if ( tmp == NULL ) 165 { 166 fprintf( stderr, "malloc 内存不足!\n" ); 167 return -1; 168 } 169 170 memcpy( tmp, node, sizeof( node_t ) ); 171 *header = tmp; 172 } 173 else 174 { 175 h = *header; 176 while ( h ) 177 { 178 if ( NODEEQUAL( h, node ) ) 179 { 180 break; 181 } 182 p = h; 183 h = h->next; 184 } 185 186 if ( h == NULL ) 187 { 188 tmp = ( node_ptr )malloc( sizeof( node_t ) ); 189 if ( tmp == NULL ) 190 { 191 fprintf( stderr, "malloc 内存不足!\n" ); 192 return -1; 193 } 194 195 memcpy( tmp, node, sizeof( node_t ) ); 196 _list_inst( header, tmp ); 197 } 198 else 199 { 200 if ( h->g > node->g ) 201 { 202 if ( h != *header ) 203 { 204 p->next = h->next; 205 } 206 else 207 { 208 *header = h->next; 209 } 210 211 memcpy( h, node, sizeof( node_t ) ); 212 _list_inst( header, h ); 213 } 214 } 215 } 216 217 return 0; 218 } 219 220 int insert_opened( node_ptr* opened, node_ptr closed, node_ptr father, int idx, loc_ptr end ) 221 { 222 int h; 223 node_t node; 224 225 // 先创建点 226 memset( &node, 0, sizeof( node_t ) ); 227 228 node.loc.x = father->loc.x + g_aroundpts[idx].x; 229 node.loc.y = father->loc.y + g_aroundpts[idx].y; 230 231 if ( node.loc.x < 0 || node.loc.x >= WORLDSIZE || \ 232 node.loc.y < 0 || node.loc.y >= WORLDSIZE || \ 233 world[node.loc.y][node.loc.x] == 1 ) 234 { 235 return 0; 236 } 237 238 if ( ( idx & 1 ) == 0 && \ 239 ( world[node.loc.y][father->loc.x] == 1 || \ 240 world[father->loc.y][node.loc.x] == 1 ) ) 241 { 242 return 0; 243 } 244 245 node.g = ( ( idx & 1 ) ? 10 : 14 ) + father->g; 246 h = get_dist( &node.loc, end ); 247 node.f = node.g + h; 248 node.father = father; 249 //node.next = NULL; 250 251 // 该点是否在closed中 252 while ( closed ) 253 { 254 if ( NODEEQUAL( &node, closed ) ) 255 { 256 return 0; 257 } 258 closed = closed->next; 259 } 260 261 262 // 该点不在closed中, 更新opened 263 if ( _updt_opened( opened, &node ) == -1 ) 264 { 265 return -1; 266 } 267 268 return 0; 269 } 270 271 272 int main() 273 { 274 int i; 275 node_ptr opened, closed; 276 node_ptr curnode; 277 //创建地图 278 //..................... 279 280 281 // 创建闭集 282 if ( ( closed = create_closed( &g_start, &g_end ) ) == NULL ) 283 { 284 return -1; 285 } 286 287 // 创建开集 288 opened = NULL; 289 290 291 // 起始点 292 curnode = closed; 293 294 // 运算 295 while ( !NODEEQUAL( &curnode->loc, &g_end ) ) //终止条件 296 { 297 // 从当前点更新开集 298 for ( i = 0; i < 8; i ++ ) 299 { 300 if ( insert_opened( &opened, closed, curnode, i, &g_end ) == -1 ) 301 { 302 goto err_exit; 303 } 304 } 305 306 // 从开集中找到当前最小值, 并加入闭集 307 if ( opened == NULL ) 308 { 309 fprintf( stdout, "无路\n" ); 310 goto err_exit; 311 } 312 curnode->next = opened; 313 opened = opened->next; 314 curnode = curnode->next; 315 curnode->next = NULL; 316 } 317 318 print_path( curnode ); 319 320 fprintf( stdout, "\ntotal = %d\n", curnode->g ); 321 322 list_destroy( opened ); 323 list_destroy( closed ); 324 325 return 0; 326 err_exit: 327 list_destroy( opened ); 328 list_destroy( closed ); 329 return -1; 330 }
一个GUI演示程序ASTARGUI