├── DebugFunc.h ├── PathFinding.cpp ├── PathFinding.h ├── README.md └── main.cpp /DebugFunc.h: -------------------------------------------------------------------------------- 1 | #ifndef _DEBUGFUNC_H_ 2 | #define _DEBUGFUNC_H_ 3 | 4 | #define MYDEBUG 1 5 | #if MYDEBUG 6 | #include 7 | #define PRINT(str, ...) fprintf(stdout, (str), ##__VA_ARGS__) 8 | #else 9 | #define PRINT(str, ...) ((void)0) 10 | 11 | #endif 12 | 13 | 14 | #endif 15 | -------------------------------------------------------------------------------- /PathFinding.cpp: -------------------------------------------------------------------------------- 1 | #include "PathFinding.h" 2 | 3 | SGridInfo **gMap = NULL; //地图; 4 | 5 | float gCurWinWidth = WINDOW_WIDTH; 6 | float gCurWinHeight = WINDOW_HEIGHT; 7 | int gClickDownX = -1; //上次鼠标按下的位置;像素坐标, -1表示无鼠标点击事件; 8 | int gClickDownY = -1; 9 | SCoordinate gDest(0, 0); //终点位置; 10 | SMoveObject gObjectPosition[MOVE_OBJECT_NUM];//移动目标的位置; 11 | 12 | void SetObstocle(int x, int y) 13 | { 14 | if (x < 0 || y < 0 || x >= WORLD_WIDTH/GRID_SIZE || y >= WORLD_HEIGHT/GRID_SIZE) 15 | { 16 | return; 17 | } 18 | gMap[x][y].c = INFI; 19 | gMap[x][y].d = ED_NULL; 20 | gMap[x][y].t = EGT_OBSTOCLE; 21 | } 22 | void SetDestination(const SCoordinate &d) 23 | { 24 | if (d.x < 0 || d.y < 0 || d.x >= WORLD_WIDTH/GRID_SIZE || d.y >= WORLD_HEIGHT/GRID_SIZE) 25 | { 26 | return; 27 | } 28 | gMap[gDest.x][gDest.y].t = EGT_NORMAL; 29 | gMap[gDest.x][gDest.y].pl = 0; 30 | gMap[d.x][d.y].d = ED_NULL; 31 | gMap[d.x][d.y].t = EGT_DESTINATION; 32 | gDest = d; 33 | } 34 | void RecoverGridType() 35 | { 36 | for (int x = 0; x < WORLD_WIDTH/GRID_SIZE; ++x) 37 | { 38 | for (int y = 0; y < WORLD_HEIGHT/GRID_SIZE; ++y) 39 | { 40 | if (EGT_DESTINATION != gMap[x][y].t && EGT_OBSTOCLE != gMap[x][y].t) 41 | { 42 | gMap[x][y].pl = INFI; 43 | gMap[x][y].t = EGT_NORMAL; 44 | } 45 | } 46 | } 47 | gMap[gDest.x][gDest.y].pl = 0; 48 | } 49 | 50 | string Num2String(int i){ 51 | stringstream ss; 52 | ss << i; 53 | return ss.str(); 54 | } 55 | void DrawString(const string &strn) { 56 | static int isFirstCall = 1; 57 | static GLuint lists; 58 | const char *str = strn.c_str(); 59 | if (isFirstCall) { // 如果是第一次调用，执行初始化; 60 | // 为每一个ASCII字符产生一个显示列表; 61 | isFirstCall = 0; 62 | 63 | // 申请MAX_CHAR个连续的显示列表编号; 64 | lists = glGenLists(MAX_CHAR); 65 | 66 | // 把每个字符的绘制命令都装到对应的显示列表中; 67 | wglUseFontBitmaps(wglGetCurrentDC(), 0, MAX_CHAR, lists); 68 | } 69 | // 调用每个字符对应的显示列表，绘制每个字符; 70 | for (; *str != '\0'; ++str) 71 | glCallList(lists + *str); 72 | } 73 | SPoint Index2World(const SCoordinate &idx) 74 | { 75 | float x = idx.x * GRID_SIZE + GRID_SIZE / 2; 76 | float y = idx.y * GRID_SIZE + GRID_SIZE / 2; 77 | x = min(x, WORLD_WIDTH); 78 | x = max(x, 0); 79 | y = min(y, WORLD_HEIGHT); 80 | y = max(y, 0); 81 | 82 | return SPoint(x, y); 83 | } 84 | SPoint Pixel2World(const SPoint &pixel) 85 | { 86 | float x = pixel.x / gCurWinWidth * WORLD_WIDTH; 87 | float y = pixel.y / gCurWinHeight * WORLD_HEIGHT; 88 | x = min(x, WORLD_WIDTH); 89 | x = max(x, 0); 90 | y = min(y, WORLD_HEIGHT); 91 | y = max(y, 0); 92 | y = abs(y - WORLD_HEIGHT); 93 | 94 | return SPoint(x, y); 95 | } 96 | SCoordinate World2Index(const SPoint &p) 97 | { 98 | int x = p.x / GRID_SIZE; 99 | int y = p.y / GRID_SIZE; 100 | x = min(x, WORLD_WIDTH/GRID_SIZE - 1); 101 | x = max(x, 0); 102 | y = min(y, WORLD_HEIGHT/GRID_SIZE - 1); 103 | y = max(y, 0); 104 | 105 | return SCoordinate(x, y); 106 | } 107 | 108 | void InitMap(int hGridNum, int vGridNum) 109 | { 110 | srand((unsigned)time(NULL)); 111 | if (NULL == gMap) 112 | { 113 | gMap = new SGridInfo*[hGridNum]; 114 | for (int x = 0; x < hGridNum; ++x) 115 | { 116 | gMap[x] = new SGridInfo[vGridNum]; 117 | for (int y = 0; y < vGridNum; ++y) 118 | { 119 | gMap[x][y].d = ED_D; 120 | //gMap[x][y].c = rand() % 10 + 15; //非均匀地图，每个节点的代价范围（15~25）; 121 | gMap[x][y].c = 20; //均匀地图; 122 | } 123 | } 124 | 125 | // 设置障碍物; 126 | for (int x = hGridNum/5; x <= hGridNum*4/5; ++x) 127 | { 128 | SetObstocle(x, vGridNum/2); 129 | } 130 | for (int y = vGridNum/4; y <= vGridNum*3/4; ++y) 131 | { 132 | SetObstocle(hGridNum/2, y); 133 | } 134 | 135 | SetDestination(gDest); 136 | CalcFlowField(gDest, hGridNum, vGridNum); 137 | InitMoveObject(); 138 | } 139 | } 140 | void InitMoveObject() 141 | { 142 | for (int i = 0; i < MOVE_OBJECT_NUM; ++i) 143 | { 144 | gObjectPosition[i].p.x = rand() % (int)WORLD_WIDTH; 145 | gObjectPosition[i].p.y = rand() % (int)WORLD_HEIGHT; 146 | gObjectPosition[i].s = rand() % 10 + 5; 147 | } 148 | } 149 | void ReleaseMap(int hGridNum) 150 | { 151 | if (NULL != gMap) 152 | { 153 | for (int i = 0; i < hGridNum; ++i) 154 | { 155 | delete []gMap[i]; 156 | } 157 | delete []gMap; 158 | gMap = NULL; 159 | } 160 | } 161 | void PathFindDisplay() 162 | { 163 | int hGridNum = WORLD_WIDTH/GRID_SIZE; //水平和垂直方向网格数; 164 | int vGridNum = WORLD_HEIGHT/GRID_SIZE; 165 | 166 | InitMap(hGridNum, vGridNum); 167 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 168 | glMatrixMode(GL_PROJECTION); 169 | DrawMap(hGridNum, vGridNum); 170 | DrawObstacle(hGridNum, vGridNum); 171 | DrawDestination(Index2World(gDest)); 172 | DrawFlowField(hGridNum, vGridNum); 173 | DrawMoveObject(hGridNum, vGridNum); 174 | glFlush(); 175 | glutSwapBuffers(); 176 | } 177 | 178 | void DrawMap(int hGridNum, int vGridNum) 179 | { 180 | //设置颜色; 181 | glClear(GL_COLOR_BUFFER_BIT); 182 | glColor3f(0.5f, 0.9f, 0.89f); 183 | 184 | // 绘制基础网格; 185 | GLfloat lineWidth = 0.5f; 186 | GLfloat xCoor = 0.0f; 187 | GLfloat yCoor = 0.0f; 188 | glLineWidth(lineWidth); 189 | glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); 190 | glBegin(GL_QUADS); 191 | for (int x = 0; x < hGridNum; ++x) 192 | { 193 | for (int y = 0; y < vGridNum; ++y) 194 | { 195 | glVertex2f(xCoor, yCoor); 196 | glVertex2f(xCoor + GRID_SIZE, yCoor); 197 | glVertex2f(xCoor + GRID_SIZE, yCoor + GRID_SIZE); 198 | glVertex2f(xCoor, yCoor + GRID_SIZE); 199 | yCoor += GRID_SIZE; 200 | } 201 | xCoor += GRID_SIZE; 202 | yCoor = 0.0f; 203 | } 204 | glEnd(); 205 | } 206 | void DrawObstacle(int hGridNum, int vGridNum) 207 | { 208 | for (int x = 0; x < hGridNum; ++x) 209 | { 210 | for (int y = 0; y < vGridNum; ++y) 211 | { 212 | if (gMap[x][y].c == INFI) 213 | { 214 | DrawLineSurroundQuads(SCoordinate(x, y)); 215 | } 216 | } 217 | } 218 | } 219 | void DrawDestination(const SPoint &dIdx) 220 | { 221 | DrawPoint(dIdx, 7, SColorRGB(0.0f, 0.0f, 1.0f)); 222 | } 223 | void DrawFlowField(int hGridNum, int vGridNum) 224 | { 225 | for (int x = 0; x < hGridNum; ++x) 226 | { 227 | for (int y = 0; y < vGridNum; ++y) 228 | { 229 | SPoint sp = Index2World(SCoordinate(x, y)); 230 | switch(gMap[x][y].d) 231 | { 232 | case ED_U: 233 | DrawArraw(sp, SPoint(sp.x, sp.y + GRID_SIZE/2)); 234 | break; 235 | case ED_D: 236 | DrawArraw(sp, SPoint(sp.x, sp.y - GRID_SIZE/2)); 237 | break; 238 | case ED_L: 239 | DrawArraw(sp, SPoint(sp.x - GRID_SIZE/2, sp.y)); 240 | break; 241 | case ED_R: 242 | DrawArraw(sp, SPoint(sp.x + GRID_SIZE/2, sp.y)); 243 | break; 244 | case ED_UL: 245 | DrawArraw(sp, SPoint(sp.x - GRID_SIZE/2, sp.y + GRID_SIZE/2)); 246 | break; 247 | case ED_UR: 248 | DrawArraw(sp, SPoint(sp.x + GRID_SIZE/2, sp.y + GRID_SIZE/2)); 249 | break; 250 | case ED_DL: 251 | DrawArraw(sp, SPoint(sp.x - GRID_SIZE/2, sp.y - GRID_SIZE/2)); 252 | break; 253 | case ED_DR: 254 | DrawArraw(sp, SPoint(sp.x + GRID_SIZE/2, sp.y - GRID_SIZE/2)); 255 | break; 256 | default: 257 | break; 258 | } 259 | } 260 | } 261 | } 262 | void DrawMoveObject(int hGridNum, int vGridNum) 263 | { 264 | for (int i = 0; i < MOVE_OBJECT_NUM; ++i) 265 | { 266 | DrawPoint(gObjectPosition[i].p, 4, SColorRGB(0.0f, 1.0f, 1.0f)); 267 | } 268 | } 269 | 270 | void DrawQuads(const SPoint &ldp, const SPoint &urp, const SColorRGB &c, int mode) 271 | { 272 | glColor3f(c.r, c.g, c.b); 273 | glLineWidth(1.0f); 274 | glPolygonMode(GL_FRONT_AND_BACK, mode); 275 | glBegin(GL_QUADS); 276 | glVertex2f(ldp.x, ldp.y); 277 | glVertex2f(ldp.x, urp.y); 278 | glVertex2f(urp.x, urp.y); 279 | glVertex2f(urp.x, ldp.y); 280 | glEnd(); 281 | } 282 | void DrawLineSurroundQuads(const SCoordinate &idx) 283 | { 284 | if (idx.x < 0 || idx.y < 0) 285 | { 286 | return; 287 | } 288 | int x = idx.x * GRID_SIZE; 289 | int y = idx.y * GRID_SIZE; 290 | 291 | DrawQuads(SPoint(x, y), SPoint(x + GRID_SIZE, y + GRID_SIZE), 292 | SColorRGB(1.0f, 0.0f, 0.0f), GL_FILL); 293 | 294 | DrawQuads(SPoint(x, y), SPoint(x + GRID_SIZE, y + GRID_SIZE), 295 | SColorRGB(0.0f, 0.0f, 0.0f), GL_LINE); 296 | } 297 | void DrawPoint(const SPoint &p, GLint size, const SColorRGB &c) 298 | { 299 | glColor3f(c.r, c.g, c.b); 300 | glPointSize(size); 301 | glBegin(GL_POINTS); 302 | glVertex2f(p.x, p.y); 303 | glEnd(); 304 | } 305 | void DrawArraw(const SPoint &sp, const SPoint &ep) //穿入世界坐标; 306 | { 307 | DrawPoint(sp, 2, SColorRGB(0.5f, 0.1f, 0.3f)); 308 | 309 | glColor3f(0.5f, 0.5f, 0.5f); 310 | glLineWidth(1.0f); 311 | glBegin(GL_LINES); 312 | glVertex2f(sp.x, sp.y); 313 | glVertex2f(ep.x, ep.y); 314 | glEnd(); 315 | } 316 | 317 | void TimeerFunc(int value) 318 | { 319 | ChangeObjectPosition(); 320 | PathFindDisplay(); 321 | glutTimerFunc(40, TimeerFunc, 1); 322 | } 323 | void MouseClick(int button, int state, int x, int y) 324 | { 325 | if (button == GLUT_LEFT_BUTTON) 326 | { 327 | SCoordinate ci; //点击位置的map坐标; 328 | static SPoint cp; //点击下时位置的像素点位置; 329 | switch (state) 330 | { 331 | case GLUT_DOWN: 332 | gClickDownX = cp.x = x; 333 | gClickDownY = cp.y = y; 334 | #if MYDEBUG 335 | ci = World2Index(Pixel2World(SPoint(x, y))); //网格横纵坐标; 336 | PRINT("Mouse clicked down point (%d,%d):(%d,%d)\n",x, y, ci.x, ci.y); 337 | #endif 338 | break; 339 | 340 | case GLUT_UP: 341 | ci = World2Index(Pixel2World(SPoint(x, y))); //网格横坐标; 342 | PRINT("Mouse clicked up point (%d,%d):(%d,%d)\n",x, y, ci.x, ci.y); 343 | if (abs(x - cp.x) < GRID_SIZE*gCurWinWidth/WORLD_WIDTH*1.0/2.0 && 344 | abs(y - cp.y) < GRID_SIZE*gCurWinHeight/WORLD_HEIGHT*1.0/2.0 345 | ) //如果鼠标拖拽的偏移量<1/2个网格，则为重置目标点; 346 | { 347 | if (gMap[ci.x][ci.y].c != INFI) 348 | { 349 | //标记终点; 350 | glClear(GL_COLOR_BUFFER_BIT); 351 | SetDestination(ci); 352 | CalcFlowField(ci, WORLD_WIDTH/GRID_SIZE, WORLD_HEIGHT/GRID_SIZE); 353 | PathFindDisplay(); //绘制所有物体; 354 | glFlush(); 355 | } 356 | } 357 | gClickDownY = gClickDownX = cp.x = cp.y = -1; 358 | break; 359 | 360 | default: 361 | break; 362 | } 363 | } 364 | } 365 | void MouseMove(int x, int y) 366 | { 367 | if (-1 == gClickDownY && -1 == gClickDownX) 368 | { 369 | return; 370 | } 371 | 372 | SCoordinate dIdx = World2Index(Pixel2World(SPoint(x, y))); //当前鼠标位置对应的map坐标; 373 | bool drawObs = false; 374 | if (abs(x - gClickDownX) >= GRID_SIZE*gCurWinWidth/WORLD_WIDTH*1.0/2.0) //x方向滑动; 375 | { 376 | int derction = (x - gClickDownX)/abs(x - gClickDownX); 377 | gClickDownX = x + derction*GRID_SIZE*gCurWinWidth/WORLD_WIDTH*1.0/2.0; 378 | gClickDownX = max(gClickDownX, 0); 379 | gClickDownX = min(gClickDownX, gCurWinWidth); 380 | drawObs = true; 381 | } 382 | if (abs(y - gClickDownY) >= GRID_SIZE*gCurWinHeight/WORLD_HEIGHT*1.0/2.0) //y方向滑动; 383 | { 384 | int direction = (y - gClickDownY)/abs(y - gClickDownY); 385 | gClickDownY = y + direction*GRID_SIZE*gCurWinHeight/WORLD_HEIGHT*1.0/2.0; 386 | gClickDownY = max(gClickDownY, 0); 387 | gClickDownY = min(gClickDownY, gCurWinHeight); 388 | drawObs = true; 389 | } 390 | if (drawObs) 391 | { 392 | if (EGT_CLOSE == gMap[dIdx.x][dIdx.y].t) 393 | { 394 | SetObstocle(dIdx.x, dIdx.y); 395 | CalcFlowField(gDest, WORLD_WIDTH/GRID_SIZE, WORLD_HEIGHT/GRID_SIZE); 396 | PathFindDisplay(); 397 | glFlush(); 398 | } 399 | } 400 | } 401 | 402 | void Initial() 403 | { 404 | glClear(GL_COLOR_BUFFER_BIT); 405 | glClearColor(1.0f, 1.0f, 1.0f, 1.0f); //清屏颜色; 406 | glMatrixMode(GL_PROJECTION); 407 | glLoadIdentity(); //回到原点; 408 | gluOrtho2D(0.0, WORLD_WIDTH, 0.0, WORLD_HEIGHT); //投影到裁剪窗大小：世界; 409 | } 410 | void ReshapeWin(int w, int h) 411 | { 412 | GLfloat aspectRatio=(GLfloat)w/(GLfloat)h; 413 | if(w <= h) 414 | { 415 | gluOrtho2D(0.0, WORLD_WIDTH, 0.0, WORLD_HEIGHT/aspectRatio); 416 | } 417 | else 418 | { 419 | gluOrtho2D(0.0, WORLD_WIDTH*aspectRatio, 0.0, WORLD_HEIGHT); 420 | } 421 | if (w != gCurWinWidth) 422 | { 423 | gCurWinWidth = w; 424 | gCurWinWidth = max(1, gCurWinWidth); 425 | } 426 | if (h != gCurWinHeight) 427 | { 428 | gCurWinHeight = h; 429 | gCurWinHeight = max(1, gCurWinHeight); 430 | } 431 | } 432 | SPoint JumpToSuitablePos(const SCoordinate &curIdx) //根据该点周围的障碍物情况，决定移动目标可跳跃的位置; 433 | { 434 | return SPoint(rand() % 6 - 3, rand() % 6 - 3); //实际功能有待后续完成; 435 | } 436 | void ChangeObjectPosition() 437 | { 438 | if (NULL == gMap) 439 | { 440 | return; 441 | } 442 | JumpToSuitablePos(SCoordinate(1,1)); 443 | for (int i = 0; i < MOVE_OBJECT_NUM; ++i) 444 | { 445 | SCoordinate op = World2Index(gObjectPosition[i].p); 446 | float moveSpeed = CALC_MOVE_SPEED(gObjectPosition[i], op); 447 | switch (gMap[op.x][op.y].d) 448 | { 449 | case ED_U: 450 | gObjectPosition[i].p.y += moveSpeed; 451 | break; 452 | case ED_D: 453 | gObjectPosition[i].p.y -= moveSpeed; 454 | break; 455 | case ED_L: 456 | gObjectPosition[i].p.x -= moveSpeed; 457 | break; 458 | case ED_R: 459 | gObjectPosition[i].p.x += moveSpeed; 460 | break; 461 | case ED_UL: 462 | gObjectPosition[i].p.x -= moveSpeed; 463 | gObjectPosition[i].p.y += moveSpeed; 464 | break; 465 | case ED_UR: 466 | gObjectPosition[i].p.x += moveSpeed; 467 | gObjectPosition[i].p.y += moveSpeed; 468 | break; 469 | case ED_DL: 470 | gObjectPosition[i].p.x -= moveSpeed; 471 | gObjectPosition[i].p.y -= moveSpeed; 472 | break; 473 | case ED_DR: 474 | gObjectPosition[i].p.x += moveSpeed; 475 | gObjectPosition[i].p.y -= moveSpeed; 476 | break; 477 | default: //当到达某个无方向的点，则在该点周围跳动; 478 | { 479 | SPoint offset = JumpToSuitablePos(op); 480 | gObjectPosition[i].p.x += offset.x; 481 | gObjectPosition[i].p.y += offset.y; 482 | break; 483 | 484 | } 485 | } 486 | gObjectPosition[i].p.x = max(1, gObjectPosition[i].p.x); 487 | gObjectPosition[i].p.x = min(WORLD_WIDTH, gObjectPosition[i].p.x); 488 | gObjectPosition[i].p.y = max(1, gObjectPosition[i].p.y); 489 | gObjectPosition[i].p.y = min(WORLD_HEIGHT, gObjectPosition[i].p.y); 490 | } 491 | } 492 | 493 | /*********计算流场**********/ 494 | bool IsCorner(const SCoordinate &p, const SCoordinate &s) //是否在障碍物拐角处，是则禁止斜对角穿越; 495 | { 496 | int x = p.x - s.x; 497 | int y = p.y - s.y; 498 | 499 | if (x != 0 && y != 0) //斜对角关系; 500 | { 501 | if (EGT_OBSTOCLE == gMap[s.x + x][s.y].t) 502 | { 503 | return true; 504 | } 505 | if (EGT_OBSTOCLE == gMap[s.x][s.y + y].t) 506 | { 507 | return true; 508 | } 509 | else 510 | return false; 511 | } 512 | return false; 513 | } 514 | int CalcCost(const SCoordinate &p, const SCoordinate &s) //路径代价函数计算; 515 | { 516 | int dirCost = 10 * sqrt((float)(abs(p.x-s.x)+abs(p.y-s.y))); //方向惩罚;斜对角惩罚系数大于水平或垂直方向; 517 | return gMap[p.x][p.y].pl + gMap[s.x][s.y].c + dirCost; 518 | } 519 | int ParentDirection(const SCoordinate &p, const SCoordinate &s) //通过父节点和自己的坐标计算方向; 520 | { 521 | int x = p.x - s.x; 522 | int y = p.y - s.y; 523 | 524 | return (30*x + 3*y); 525 | } 526 | void UpdateOpenList(multiset &openList, const SCoordinate &cneterIdx) 527 | { 528 | int sx, sy, ex, ey; 529 | sx = max(cneterIdx.x - 1, 0); 530 | sy = max(cneterIdx.y - 1, 0); 531 | ex = min(cneterIdx.x + 1, WORLD_WIDTH/GRID_SIZE - 1); 532 | ey = min(cneterIdx.y + 1, WORLD_HEIGHT/GRID_SIZE - 1); 533 | for (int x = sx; x <= ex; ++x) 534 | { 535 | for (int y = sy; y <= ey; ++y) 536 | { 537 | SGridInfo &curGrid = gMap[x][y]; 538 | if (EGT_NORMAL == curGrid.t) //普通节点; 539 | { 540 | if (IsCorner(cneterIdx, SCoordinate(x, y))) //是拐角的斜对角点，则跳过; 541 | { 542 | continue; 543 | } 544 | int cost = CalcCost(cneterIdx, SCoordinate(x, y)); 545 | openList.insert(SOpenGridInfo(SCoordinate(x, y), cost)); 546 | curGrid.t = EGT_OPEN; 547 | curGrid.pl = cost; 548 | curGrid.d = ParentDirection(cneterIdx, SCoordinate(x, y)); 549 | } 550 | else if (EGT_CLOSE == curGrid.t) 551 | { 552 | int cost = CalcCost(cneterIdx, SCoordinate(x, y)); 553 | if (cost < curGrid.pl) 554 | { 555 | curGrid.pl = cost; 556 | curGrid.d = ParentDirection(cneterIdx, SCoordinate(x, y)); 557 | } 558 | } 559 | } 560 | } 561 | } 562 | void CalcFlowField(const SCoordinate &d, int hGridNum, int vGridNum) 563 | { 564 | RecoverGridType(); 565 | multiset openList; //有序多重集合，升序排列; 566 | openList.insert(SOpenGridInfo(d, gMap[d.x][d.y].c)); 567 | SCoordinate curIdx = d; //当前选中的最优路径节点; 568 | while (!openList.empty()) 569 | { 570 | openList.erase(openList.begin()); //当前点加入closeList; 571 | gMap[curIdx.x][curIdx.y].t = EGT_CLOSE; 572 | 573 | UpdateOpenList(openList, curIdx); //更新当前点周围的点到OpenList; 574 | 575 | if (openList.empty()) 576 | { 577 | break; 578 | } 579 | curIdx = openList.begin()->c; //选择当前openlist中最优的路径点; 580 | } 581 | gMap[gDest.x][gDest.y].t = EGT_DESTINATION; //恢复终点的状态; 582 | } 583 | -------------------------------------------------------------------------------- /PathFinding.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChirlChen/Flow-Field-PathFinding/dd033e0076c40f3daca385cb8a48818a40c13aaa/PathFinding.h -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 流场寻路算法简单实现 2 | Flow field pathfinding algorithm implementation 3 | * key words: `OpenGl` `Dijkstra` `Flow Field` `Path Finding`
4 | Flow field pathfinding algorithm implementation 5 | 6 | * **当前实现的主要效果：**
7 | 1.地图上随机分布200个移动目标；
8 | 2.移动目标可绕过障碍物，沿着地图的流场找到最短路径（最小代价的路径）到达目标点；
9 | 3.可通过点击地图上的非障碍物区域，实时的更改目标地点，并更新流场；
10 | 4.可通过按着鼠标左键在地图上拖拽，实时绘制障碍物，并动态更新流场；
11 | 12 | * **效果图：**
13 | 蓝色点为终点，红色块为障碍物，蔚蓝色小点为移动目标，小点加线为流场方向指示箭头。这里的地图为均匀地图（即移动目标经过地图中每个网格的代价相同）
14 | 15 | 1.初始地图效果：
16 | ![这里写图片描述](http://img.blog.csdn.net/20161014172713419) 17 | 18 | 2.动态设置障碍物效果：
19 | ![这里写图片描述](http://img.blog.csdn.net/20161014172629950) 20 | 21 | 3.重置终点效果：
22 | ![这里写图片描述](http://img.blog.csdn.net/20161014195103285) 23 | * **Bug：**
24 | 由于初次使用OpenGl，还有很多细节做得不到位
25 | 1.窗口大小变换后，鼠标点击的坐标转换等功能仍未实现；
26 | 2.由于效果需要导致的，移动目标在障碍物拐角处会跳到障碍物里面，然后穿过障碍物；
27 | 3.……
28 | 29 | * **算法步骤：**
30 | 1.初始化地图；
31 | 2.使用Dijstra算法，从终点开始，逐一对最优的邻接节点计算路径，从而计算整张网格化地图上所有点到终点的最短路；
32 | 3.根据最短路形成地图流场；
33 | 34 | * **参考文章：**
35 | 主要参照了[这篇文章](https://gamedevelopment.tutsplus.com/tutorials/understanding-goal-based-vector-field-pathfinding--gamedev-9007)，思路很简单。 36 | -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include "PathFinding.h" 2 | 3 | int main(int argc, char *argv[]) 4 | { 5 | //PRINT("%d") 6 | glutInit(&argc, argv); 7 | glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB); 8 | glutInitWindowSize(WINDOW_WIDTH, WINDOW_HEIGHT); 9 | glutInitWindowPosition(300, 300); 10 | glutCreateWindow("FlowFieldPathFinding"); 11 | Initial(); 12 | glutDisplayFunc(PathFindDisplay); 13 | glutMouseFunc(MouseClick); 14 | glutMotionFunc(MouseMove); 15 | //glutReshapeFunc(ReshapeWin); 16 | glutTimerFunc(40, TimeerFunc, 1); 17 | glutMainLoop(); 18 | 19 | return 0; 20 | } 21 | --------------------------------------------------------------------------------