A星算法,找寻最短路径

本文深入探讨了A星算法的工作原理,展示了如何利用该算法有效地寻找图形中的最短路径。通过实例分析,解释了A星算法的关键步骤,包括启发式函数的选择、优先队列的使用以及节点的扩展策略。此外,还讨论了A星算法在游戏开发、路径规划等领域的重要应用。
摘要由CSDN通过智能技术生成
#include <iostream>
#include <cstdio>
#include <queue>
#include <algorithm>
#include <cmath>
#define N 1000
#define inf 1<<30;
using namespace std;
/*
	a星算法,找寻最短路径
	算法核心:有两个表open表和close表
		将方块添加到open列表中,该列表有最小的和值。且将这个方块称为S吧。
		将S从open列表移除,然后添加S到closed列表中。
		对于与S相邻的每一块可通行的方块T:
		如果T在closed列表中:不管它。
		如果T不在open列表中:添加它然后计算出它的和值。
		如果T已经在open列表中:当我们使用当前生成的路径到达那里时,检查F(指的是和值)是否更小。如果是,更新它的和值和它的前继。

		F = G + H        (G指的是从起点到当前点的距离,而H指的是从当前点到目的点的距离(移动量估算值采用曼哈顿距离方法估算)
*/
int map[6][7];     //0表示是路,1表示有阻碍物
int xstart, ystart, xend, yend;       //(x1,y1)起点,(x2, y2)目的点
int close[6][7];  //0表示不在,1表示在
int n;
void astar(int , int );
bool check(int x, int y);
int panyical(int x, int y);
void print2();

struct point{
	int x, y;
	int f, g, h;
	int prex, prey;   //上一个点的x,y	
	point(int x0, int y0, i
;;时间计算 (defun z_timer (/ stime h m s) (if (not zhf_time_dot) (setq zhf_time_dot (getvar "date") h nil ) (progn (setq stime (getvar "date")) (setq stime (- stime zhf_time_dot)) (setq stime (* 86400.0 (- stime (fix stime)))) (setq h (fix (/ stime 3600))) (setq m (fix (/ (- stime (* h 3600)) 60))) (setq s (- stime (* m 60) (* h 3600))) (setq zhf_time_dot nil) (strcat (if (> h 0) (strcat (rtos h 2 0) "小时") "" ) (if (> m 0) (strcat (rtos m 2 0) "分钟") "" ) (rtos s 2 2) "秒" ) ) ) ) ;;路径颜色标示 (defun show (lst stop) (mapcar '(lambda (x) (redraw (vlax-vla-object->ename x) 3)) lst ) (if stop (progn (getpoint) (mapcar '(lambda (x) (redraw (vlax-vla-object->ename x) 4)) lst ) ) ) ) ;;vla是真的情况下,将选择集转换成vla-object实体 ;;vla是假的情况下,将选择集转换成lisp实体 (defun ss2lst (ss vla / re e) (if ss (repeat (setq n (sslength ss)) (if vla (setq e (vlax-ename->vla-object (ssname ss (setq n (1- n))))) (setq e (ssname ss (setq n (1- n)))) ) (setq re (append re (list e))) ) ) re ) ;;获得点所在位置的线(line,pline,spline)、圆弧、椭圆弧 (defun getss@ (p) (ssget "c" p (polar p (/ pi 4) (/ (getvar "viewsize") 5000)) '((0 . "arc,ellipse,*line")) ) ) ;;获得线段另一端连接实体 (defun getconnect (e) (vl-remove e (append (ss2lst (getss@ (vlax-curve-getStartpoint e)) t) (ss2lst (getss@ (vlax-curve-getEndpoint e)) t) ) ) ) ;;除去中的重复项,本例程未用到此函数 (defun remove:same (lst / re) (foreach n lst (if (member n re) () (setq re (append re (list re))) ) ) re ) ;;获得实体的长度 (defun get:len (e) (vlax-curve-getDistAtParam e (vlax-curve-getEndParam e)) ) ;;获得实体的另一个端点到终点的距离 (defun dist-other (ent pt / pta ptb pta2 ptb2) (setq pta (vlax-curve-getStartpoint ent)) (setq pta2 (list (fix (* 10 (car pta)))(fix (* 10 (cadr pta))))) (setq ptb (vlax-curve-getEndpoint ent)) (if (member pta2 pt-list) (distance ptb pt) (distance pta pt) ) ) ;;获得实体的另一个端点 (defun getotherpt (ent / pta ptb pta2 ptb2) (setq pta (vlax-curve-getStartpoint ent)) (setq pta2 (list (fix (* 10 (car pta)))(fix (* 10 (cadr pta))))) (setq ptb (vlax-curve-getEndpoint ent)) (if (member pta2 pt-list) (setq pt-other ptb) (setq pt-other pta) ) ) ;;;________________________________________________ ;;;________________________________________________ ;;;________________________________________________ ;;;________________________________________________ ;;;A算法 (defun main (pt1 pt2 show / ss sse line path paths shortlen shortlst ss1 shortest) (setq count 0) (setq ss (ss2lst (getss@ pt1) t) sse (ss2lst (getss@ pt2) t) ) (if (and ss sse) (progn (setq passed-ss ss ;;起点处的实体,作为延伸后获得的实体中要去除的实体 pt-list (list (list (fix (* 10 (car pt1)))(fix (* 10 (cadr pt1))))) path-ss (mapcar '(lambda (x) (list x)) ss) ;;;路径 dist-ss (mapcar '(lambda (x) (list x (get:len x)(dist-other x pt2))) ss) ;;;路径,带长度 dist-ss (vl-sort dist-ss '(lambda (a b) (< (+ (cadr a)(caddr a)) (+ (cadr b)(caddr b))))) ;;;排序后 complete nil ) (if complete (setq complete (vl-sort complete '(lambda (a b) (< (cadr a) (cadr b))) ) shortest (cadar complete) ;;shortest最短路径 ) ) (if (and shortest (= shortest (distance pt1 pt2))) (progn ;;起止点有直接联通,并且是直线连接(mapcar '(lambda (x) (if (member x sse);;判断起止点之间是否有直接的连接 (setq complete (append complete (list (list x (get:len x)))) ) ) ) ss ) (if (and shortest (= shortest (distance pt1 pt2))) (progn ;;起止点有直接联通,并且是直线连接 (list (cadar complete) (list (caar complete))) ) (progn ;;起止点有直接联通,但不是直线连接 或 没有直接连通 (while (and dist-ss (> (length sse) (length complete))) (setq now (car dist-ss) dist-ss (cdr dist-ss) ) ;;;_____________________________ ;;;_____________________________ ;;;_____________________________ (if show (progn (vlax-put (car now) 'color (+ 21 (* 10 (rem count 20)))) (vla-update (car now)) ) ) ;;;_____________________________ ;;;_____________________________ ;;;_____________________________ (if (member (car now) sse) (progn (setq complete (append complete (list now))) ;;;__________________________________________________ ;;;到达终点后剔出所有距离已经超出最小路由长度的未完成方向 (setq complete (vl-sort complete '(lambda (a b) (< (cadr a) (cadr b))) ) ) (setq shortest (cadar complete)) (setq dist-ss (mapcar '(lambda (x) (if (< (cadr x) shortest) x nil ) ) dist-ss ) ) (setq dist-ss (vl-remove nil dist-ss)) ;;;__________________________________________________ ;;;__________________________________________________ ) (progn (setq count (1+ count)) (setq pt-other (getotherpt (car now))) (setq ss (ss2lst (getss@ pt-other) t)) (setq pt-list (cons (list (fix (* 10 (car pt-other)))(fix (* 10 (cadr pt-other)))) pt-list)) ;; (mapcar '(lambda (x) (setq ss (vl-remove x ss))) ;; passed-ss ;; ) ;;上句替换为下面一句 passed-ss较长时,不如直接处理ss (foreach n ss ;;去掉已走过的路径 (if (member n passed-ss) (setq ss (vl-remove n ss)) ) ) (if ss (progn ;; (setq passed-ss (append passed-ss ss) ;; path-ss (append ;; path-ss ;; (mapcar '(lambda (x) (list x (car now))) ss) ;; ) ;; ) ;; (setq dist-ss (append ;; dist-ss ;; (mapcar ;; '(lambda (x) ;; (if (or (not shortest) ;; (< (get:len x) shortest) ;; ) ;; (list x (+ (cadr now) (get:len x))) ;; ) ;; ) ;; ss ;; ) ;; ) ;; ) ;;上两句替换为下面循环结构 mapcar+append->foreach+cons (foreach n ss (setq passed-ss (cons n passed-ss)) ;;把新路径增加到已走过的路径 (setq path-ss (cons (list n (car now)) path-ss)) ;;把新路由增加到已有路由中 (if (or (not shortest) (< (get:len n) shortest)) (setq dist-ss (cons (list n (+ (cadr now) (get:len n))(dist-other n pt2)) dist-ss)) ) ) ;; (setq dist-ss (vl-remove nil dist-ss)) (setq dist-ss (vl-sort dist-ss ;;按距离排序 '(lambda (a b) (< (+ (cadr a)(caddr a)) (+ (cadr b)(caddr b)))) ) ) ) ) ) ) ) ;;;_____________________________ ;;;_____________________________ ;;;_____________________________ (if show (progn (mapcar '(lambda (x) (vlax-put x 'color 0)) passed-ss) (mapcar '(lambda (x) (vla-update x)) passed-ss) ) ) ;;;_____________________________ ;;;_____________________________ ;;;_____________________________ (if complete (progn (setq complete (vl-sort complete '(lambda (a b) (< (cadr a) (cadr b))) ) n (car complete) ) (setq len (cadr n) n (car n) ) (while n (setq ss1 (append ss1 (list n))) (setq n (cadr (assoc n path-ss))) ) (list len (reverse ss1)) ) nil ) ) ) ) nil ) ) ;;;________________________________________________ ;;;________________________________________________ ;;;________________________________________________ ;;;________________________________________________ (defun c:ttz (/ pt1 pt2 ss1 ss2 complete) (redraw) (setq pt1 (getpoint "\n起点:") pt2 (getpoint "\n终点:") ) (mapcar '(lambda (pt) (grdraw (polar pt (* pi 0.25) (/ (getvar "viewsize") 40)) (polar pt (* pi -0.75) (/ (getvar "viewsize") 40)) 1 ) (grdraw (polar pt (* pi 0.75) (/ (getvar "viewsize") 40)) (polar pt (* pi -0.25) (/ (getvar "viewsize") 40)) 1 ) ) (list pt1 pt2) ) (setq zhf_time_dot nil) (z_timer) (setq ss1 (main pt1 pt2 t)) (if ss1 (progn (setq ss2 (ssadd)) (mapcar '(lambda (x) (setq ss2 (ssadd (vlax-vla-object->ename x) ss2)) ) (cadr ss1) ) (princ (strcat "\n虚线显示最短路线, 共需" (itoa (sslength ss2)) "步,总长度为:" (rtos (car ss1)) " 历时:" (z_timer) ) ) (show (cadr ss1) nil) ) (princ (strcat "\n两点间没有可连通路径,历时:" (z_timer))) ) (princ) ) ;;;________________________________________________ ;;;________________________________________________ ;;;________________________________________________ ;;;________________________________________________ (princ "\n寻找连接两点的最近路线,by wkai @ xdcad ") (princ "\n前提 所有路线只在交点处交叉,起点和终点选择路线的端点." ) (princ "\n核心函数 (main 起点 终点 是否显示搜索过程) ") (princ "\n返回值 (最短路线长度 最短路线途径实体)") (princ "\n测试命令:ttz\n") (princ)
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值