一饭多吃.jpg
放个最初版本,A*求最短路
#include "Logger.h"
#include "ColorUtils.h"
#include "ImgUtils.h"
#include<graphics.h>
#include<windows.h>
#include<iostream>
#include<conio.h>
#include<cstdio>
#include<cmath>
#include<queue>
#include <thread>
#define DIS_PERCENT 1.2
#define WALK_PERCENT 65
#define INF 1000
using namespace std;
unsigned int unWidth = 0, unHeight = 0;
const double sqrt2 = sqrt(2.0);
IMAGE Map;
int lu[2333][2333], vis[2333][2333];
double dis[2333][2333];
string lastInf;
bool selected = false;
void mapToSpot()
{
memset(lu, 0, sizeof(lu));
for (int i = 0; i < unWidth; ++i)
for (int j = 0; j < unHeight; ++j)
if (ColorUtils::isTarget(i, j))lu[i][j] = 1;
}
struct dian
{
int x, y;
double dis_from, dis_to;
friend bool operator <(const dian& a, const dian& b)
{
return a.dis_from + a.dis_to > b.dis_from + b.dis_to;
}
}from[2333][2333];//用来找回来的路径
priority_queue<dian>q;
int dx[10] = { 0,-1,-1,1,1,-1,0,0,1 };
int dy[10] = { 0,-1,1,-1,1,0,-1,1,0 };
void DIJ(int X1, int Y1, int X2, int Y2)
{
while (!q.empty())q.pop();
memset(from, 0, sizeof(from));
memset(vis, 0, sizeof(vis));
for (int i = 0; i < unWidth; ++i)
for (int j = 0; j < unHeight; ++j) dis[i][j] = 114514;
dian now, to; double tmp_dis; now.x = X1; now.y = Y1; now.dis_from = 0;
dis[now.x][now.y] = 0;
q.push(now);
bool flag = false;
while (!q.empty() && !flag)
{
now = q.top(); q.pop();
if (vis[now.x][now.y])continue; vis[now.x][now.y] = 1;
setfillcolor(RGB(255, 0, 0));
solidcircle(now.x, now.y, 1);
for (int i = 1; i <= 8; ++i)
{
to.x = now.x + dx[i]; to.y = now.y + dy[i];
if (i <= 4)tmp_dis = now.dis_from + sqrt2;
else tmp_dis = now.dis_from + 1.0;
if (to.x < 0 || unWidth <= to.x || to.y < 0 || unHeight <= to.y || lu[to.x][to.y] == 0 || dis[to.x][to.y] <= tmp_dis)continue;
dis[to.x][to.y] = tmp_dis; to.dis_from = tmp_dis;
//Sleep(2);//用来展示寻路过程
to.dis_to = sqrt((double)(X2 - to.x) * (X2 - to.x) + (Y2 - to.y) * (Y2 - to.y));
from[to.x][to.y] = now;
q.push(to);
if (to.x == X2 && to.y == Y2) flag = true;
}
}
now.x = X2; now.y = Y2;
int prepts[5000] = { 0 };
putimage(0, 0, &Map);
while (now.x != X1 || now.y != Y1)
{
//Sleep(2);
setfillcolor(RGB(0, 0, 255));
solidcircle(now.x, now.y, 2);
now = from[now.x][now.y];
}
}
ExMessage m1, m2, m3;
void watch()
{
if (!selected)
{
cin >> m1.x >> m1.y;
cin >> m2.x >> m2.y;
selected = true;
}
}
void showMouse()
{
while (1)
{
ExMessage m;
if (peekmessage(&m))
{
setfillcolor(BLACK);
solidrectangle(0, 0, 170, 30);
setfillcolor(RGB(255, 0, 0));
string ch = "Mouse: " + to_string(m.x) + ' ' + to_string(m.y);
if (ColorUtils::isTarget(m.x, m.y))
{
ch += " Yes";
}
else
{
ch += " No";
}
int num = MultiByteToWideChar(0, 0, ch.c_str(), -1, NULL, 0);
wchar_t* wide = new wchar_t[num];
MultiByteToWideChar(0, 0, ch.c_str(), -1, wide, num);
outtextxy(0, 0, wide);
}
}
}
int main()
{
ImgUtils::GetPicWidthHeight("./ChangZhou.png", &unWidth, &unHeight);
initgraph(unWidth, unHeight, EW_SHOWCONSOLE);
setlinestyle(PS_SOLID | PS_ENDCAP_FLAT | PS_JOIN_BEVEL, 3);
loadimage(&Map, _T(".\\ChangZhou.png"));
putimage(0, 0, &Map);
Logger::logInfo("预处理地图中...");
mapToSpot();
Logger::logSuccess("加载完毕!可以开始选点了!");
thread mouse(showMouse);
mouse.detach();
while (1)
{
selected = false;
setfillcolor(RGB(255, 0, 0));
thread input(watch);
input.detach();
while (!selected)
{
if (peekmessage(&m1))
{
if (m1.message == WM_LBUTTONDOWN && ColorUtils::isTarget(m1.x, m1.y))
{
solidcircle(m1.x, m1.y, 4);
if (m1.ctrl != true) break;
}
}
}
lastInf = "第一个点的坐标为: " + to_string(m1.x);
lastInf += " " + to_string(m1.y);
Logger::logInfo(lastInf);
while (!selected)
{
if (peekmessage(&m2))
{
if (m2.message == WM_LBUTTONDOWN && ColorUtils::isTarget(m2.x, m2.y))
{
solidcircle(m2.x, m2.y, 4);
selected = true;
break;
}
}
}
lastInf = "第二个点的坐标为: " + to_string(m2.x);
lastInf += " " + to_string(m2.y);
Logger::logInfo(lastInf);
double dur;
clock_t start, end;
start = clock();
Logger::logInfo("Starting A*...");
DIJ(m1.x, m1.y, m2.x, m2.y);
end = clock();
dur = (double)(end - start);
Logger::logSuccess("Use Time: " + to_string(dur / CLOCKS_PER_SEC));
while (1)
{
if (peekmessage(&m3)&& m3.message == WM_LBUTTONDOWN)break;
}
putimage(0, 0, &Map);
}
return 0;
}
标签:include,string,int,地图,m1,m2,导航系统,now
From: https://www.cnblogs.com/wljss/p/17431023.html