地图导航系统

一饭多吃.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;
}