在此僅提供程式碼，不對原理進行解釋。
如果想知道原理請自行百度，已經有很多前輩寫過了。
這裡用到了簡單的圖形庫 easyX

#include<iostream>
#include<math.h>
#include<graphics.h>
using namespace std;

struct Node 
{
	Node* pre;
	Node* next;
	int x;
	int y;
	int g;
	double f;
	Node(int x = 0, int y = 0, int g = 0, double f = 0, Node* node =
 
 0)
	{
		this->x = x;
		this->y = y;
		this->g = g;
		this->f = f;
		this->pre = node;
		this->next = 0;
	}
};

struct Priority_Queue
{
	Node* head;
	Priority_Queue() 
	{
		head = new Node;
	}
	void push(Node* node)
	{
		for (auto i = head; i != 0; i = i->next) 
		{
			if (i->next ==
 
 0 || i->next->f >= node->f)
			{
				Node* temp = node;
				temp->next = i->next;
				i->next = temp;
				break;
			}
		}
	}
	void pop() 
	{
		head->next = head->next->next;
	}
	bool isEmpty() 
	{
		return head->next == 0;
	}
	Node* find(int x, int y) 
	{
		for (auto i =
 
 head->next; i != 0; i = i->next)
		{
			if (i->x == x && i->y == y) 
			{
				return i;
			}
		}
		return 0;
	}
	void show() 
	{
		cout << "out come :";
		for (auto i = head->next; i != 0; i = i->next)
		{
			cout << i->f << " ";
		}
		cout << endl;
	}
};
double cost(int xOrigin, int yOrigin, int aimX, int aimY) 
{
	return sqrt((xOrigin - aimX)*(xOrigin - aimX) + (yOrigin - aimY)*(yOrigin - aimY));
}

void line(Node* node) 
{
	int j = 0;
	for (auto i = node; i != 0; i = i->pre, j++) 
	{
		putpixel(i->x, i->y, 0xffffff);
	}
	cout << "Completely :" << j << endl;
}

void addPoint(int x, int y, Node* currentNode, char visited[][768], Priority_Queue& queue, int aimX, int aimY) 
{
	COLORREF color1 = getpixel(x, y);
	int g = currentNode->g + 1;
	if (!visited[x][y])
	{
		Node* node = queue.find(x, y);
		double co = cost(x, y, aimX, aimY) + g;
		if (node == 0 && color1 != 0x00ff00 && color1 != 0x0000ff)
		{
			queue.push(new Node(x, y, g, co, currentNode));
			putpixel(x, y, 0x00ff00);
		}
		else if (node != 0 && co < node->f)
		{
			node->f = co;
			node->g = g;
			node->pre = currentNode;
		}
	}
}
typedef Node* NodeStar;
void aStart(int xOrigin, int yOrigin, int aimX, int aimY) 
{
	Priority_Queue queue;
	char visited[1024][768] = {0};
	Node* currentNode = new Node(xOrigin, yOrigin, 0, cost(xOrigin, yOrigin, aimX, aimY));
	queue.push(currentNode);
	putpixel(aimX, aimY, 0xffffff);
	cout << "aim :" << aimX << " " << aimY << endl;
	while (true) 
	{
		if (queue.isEmpty()) 
		{
			break;
		}
		
		currentNode = queue.head->next;
		visited[currentNode->x][currentNode->y] = 1;                     //訪問記錄一下
		queue.pop();
		bool isOver = currentNode->x == aimX && currentNode->y == aimY;
		if (isOver){           //尋路結束		
			line(currentNode);
			break;
		}
		addPoint
		(
			currentNode->x - 1,
			currentNode->y,
			currentNode,
			visited,
			queue,
			aimX,
			aimY
		);
		addPoint
		(
			currentNode->x + 1,
			currentNode->y,
			currentNode,
			visited,
			queue,
			aimX,
			aimY
		);
		addPoint
		(
			currentNode->x,
			currentNode->y - 1,
			currentNode,
			visited,
			queue,
			aimX,
			aimY
		);
		addPoint
		(
			currentNode->x,
			currentNode->y + 1,
			currentNode,
			visited,
			queue,
			aimX,
			aimY
		);
	}

}
int main()
{
	initgraph(800, 600, 1);
	MOUSEMSG msg;
	bool isDown = false;
	int lastX;
	int lastY;
	bool isDR = false;
	int lastRX;
	int lastRY;
	while (true) 
	{
		while (MouseHit()) 
		{
			msg = GetMouseMsg();
			//BeginBatchDraw();
			if (msg.uMsg == WM_LBUTTONDOWN) 
			{
				isDown = true;
				lastX = msg.x;
				lastY = msg.y;
			}
			else if(msg.uMsg == WM_LBUTTONUP)
			{
				isDown = false;
				aStart(lastX, lastY, msg.x, msg.y);
			}
			else if(msg.uMsg == WM_RBUTTONDOWN)
			{
				isDR = true;
				lastRX = msg.x;
				lastRY = msg.y;
			}
			else if (msg.uMsg == WM_RBUTTONUP)
			{
				isDR = false;
			}
			if (isDR) 
			{

				setcolor(0x0000ff);
				line(lastRX, lastRY, msg.x, msg.y);
				lastRX = msg.x;
				lastRY = msg.y;
			}

			//EndBatchDraw();
		}
		Sleep(50);
	}
	closegraph();
	
}

下面是執行結果
紅色是障礙物
綠色是尋路時訪問過的點
白色是最線找到的路