标题: 关于寻求最短路径的一种算法、附件为算法的C/C++实现,欢迎下载 [打印本页]
作者: ahyl    时间: 2019-5-17 11:25
标题: 关于寻求最短路径的一种算法、附件为算法的C/C++实现,欢迎下载
最短路径寻优

（以下关于Dijstra的说明，是借用算法与数据结构的发帖说明、侵权即删）原帖链接最短路径寻优如上图所示、如何寻求从 A 出发到 G 点的最短路径呢？Dijstra算法就是要求出这个最短的路径；

让我们来演示一下迪杰斯特拉的详细过程：        第1步，创建距离表。表中的Key是顶点名称，Value是从起点A到对应顶点的已知最短距离。        但是，一开始我们并不知道A到其他顶点的最短距离是多少，Value默认是无限大：

第2步，遍历起点A，找到起点A的邻接顶点B和C。从A到B的距离是5，从A到C的距离是2。把这一信息刷新到距离表当中：

第3步，从距离表中找到从A出发距离最短的点，也就是顶点C。第4步，遍历顶点C，找到顶点C的邻接顶点D和F（A已经遍历过，不需要考虑！！！！！！！！！！代码编写中就需要注意这一点）。从C到D的距离是6，所以A到D的距离是2+6=8；从C到F的距离是8，所以从A到F的距离是2+8=10。把这一信息刷新到表中：

接下来重复第3步、第4步所做的操作：第5步，也就是第3步的重复，从距离表中找到从A出发距离最短的点（C已经遍历过，不需要考虑），也就是顶点B。第6步，也就是第4步的重复，遍历顶点B，找到顶点B的邻接顶点D和E（A已经遍历过，不需要考虑）。从B到D的距离是1，所以A到D的距离是5+1=6，小于距离表中的8；从B到E的距离是6，所以从A到E的距离是5+6=11。把这一信息刷新到表中：

（在第6步，A到D的距离从8刷新到6，可以看出距离表所发挥的作用。距离表通过迭代刷新，用新路径长度取代旧路径长度，最终可以得到从起点到其他顶点的最短距离）第7步，从距离表中找到从A出发距离最短的点（B和C不用考虑），也就是顶点D。第8步，遍历顶点D，找到顶点D的邻接顶点E和F。从D到E的距离是1，所以A到E的距离是6+1=7，小于距离表中的11；从D到F的距离是2，所以从A到F的距离是6+2=8，小于距离表中的10。把这一信息刷新到表中：

第9步，从距离表中找到从A出发距离最短的点，也就是顶点E。第10步，遍历顶点E，找到顶点E的邻接顶点G。从E到G的距离是7，所以A到G的距离是7+7=14。把这一信息刷新到表中：

第11步，从距离表中找到从A出发距离最短的点，也就是顶点F。第10步，遍历顶点F，找到顶点F的邻接顶点G。从F到G的距离是3，所以A到G的距离是8+3=11，小于距离表中的14。把这一信息刷新到表中：

就这样，除终点以外的全部顶点都已经遍历完毕，距离表中存储的是从起点A到所有顶点的最短距离。显然，从A到G的最短距离是11。（路径：A-B-D-F-G）
下面附上算法的C++实现
  1. // dijstra.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
  2. //
  3. //利用状态机来描述 dijstra算法
  4. //寻求最短路径


  7. #include "pch.h"
  8. #include <iostream>

  10. using namespace std;

  12. typedef struct {
  13.         char nextPointName;
  14.         int  distance;
  15. }NEXT_POINT;

  17. typedef struct {
  18.         int                curVaule;
  19.         int                expandFlag;
  20.         char        name;
  21.         int                linkNum;
  22.         NEXT_POINT  nextPoint[5];
  23.         char        route[10] = {'A'};
  24. }POINT;

  26. POINT A = { 1000,0,'A' ,2,'B',5,'C',2};
  27. POINT B = { 1000,0,'B' ,2,'D',1,'E',6};
  28. POINT C = { 1000,0,'C' ,2,'D',6,'F',8};
  29. POINT D = { 1000,0,'D' ,2,'E',1,'F',2};
  30. POINT E = { 1000,0,'E' ,1,'G',7};
  31. POINT F = { 1000,0,'F' ,1,'G',3};
  32. POINT G = { 1000,0,'G' };


  35. void Dijkstra(POINT* startPoint, POINT* endPoint, POINT* piontArray, int pointNum);
  36. int main()
  37. {
  38.         POINT array[10] = { A,B,C,D,E,F,G };
  39.         Dijkstra(&A,&G,array,7);
  40.        
  41.         cout << "        最短路径为        "<<G.route <<endl<<"        最短路径长度为        "<< G.curVaule  << endl;
  42.         system("pause");
  43. }

  45. void Dijkstra(POINT* startPoint, POINT* endPoint, POINT* piontArray,int pointNum) {
  46.        
  47.         POINT unexpandPoint[20] = {0};
  48.        
  49.         int leftNum = pointNum;
  50.         POINT expandPoint=*(startPoint);       
  51.         int temp = 0;

  53.         for (int i = 0; i < pointNum; ++i) {
  54.                 unexpandPoint[i] = piontArray[i];
  55.         }
  56.         unexpandPoint[0].curVaule = 0;

  58.         while (1) {
  59.                 expandPoint = unexpandPoint[0];
  60.                 for (int i = 0; i < leftNum; ++i) {
  61.                         if (expandPoint.curVaule > unexpandPoint[i].curVaule) {
  62.                                 expandPoint = unexpandPoint[i];
  63.                                 temp = i;
  64.                         }
  65.                 }
  66.                 if (expandPoint.name == endPoint->name) {
  67.                         *(endPoint) = expandPoint;

  69.                         break;
  70.                 }       
  71.                 for (int i = 0; i < leftNum; ++i) {
  72.                         if (i > temp) {
  73.                                 unexpandPoint[i - 1] = unexpandPoint[i];
  74.                         }       
  75.                 }
  76.                 temp = 0;
  77.                 leftNum--;
  78.                 for (int i = 0; i < expandPoint.linkNum; ++i) {       
  79.                         for (int j = 0; j < leftNum; ++j) {
  80.                                 if (expandPoint.nextPoint[i].nextPointName == unexpandPoint[j].name) {
  81.                                         if (unexpandPoint[j].curVaule > expandPoint.nextPoint[i].distance + expandPoint.curVaule) {
  82.                                                 unexpandPoint[j].curVaule = expandPoint.nextPoint[i].distance+ expandPoint.curVaule;                                       
  83.                                         }
  84.                                         if (unexpandPoint[j].curVaule > 1000) {
  85.                                                 unexpandPoint[j].curVaule -= 1000;
  86.                                        
  87.                                         }

  89.                                         //路径更新
  90.                                         for (int k = 0; k < 10; ++k) {
  91.                                                 unexpandPoint[j].route[k] = 0;                                                               
  92.                                         }
  93.                                         for (int k = 0; k < 10; ++k) {
  94.                                                 if (expandPoint.route[k] != 0) {
  95.                                                         unexpandPoint[j].route[k] = expandPoint.route[k];
  96.                                                 }
  97.                                                 else {
  98.                                                         unexpandPoint[j].route[k] = unexpandPoint[j].name;
  99.                                                         break;
  100.                                                 }
  101.                                         }                                                       
  102.                                 }                       
  103.                         }                                       
  104.                 }               

  106.         }
  107. }
  108. #if 0
  109. ##网上查找到的关于 dajstra 算法的较精简的代码例程

  111. #include<iostream>
  112. #include<cstring>
  113. #define INF 10000000
  114. using namespace std;
  115. int temp[2005][2005], dis[1005];
  116. void dijkstra(int n)
  117. {
  118.         int i, min, flag, j, vis[2005] = { 0 };
  119.         for (i = 1; i <= n; i++) dis[i] = temp[1][i];
  120.         for (i = 1; i < n; i++)
  121.         {
  122.                 min = INF;
  123.                 flag = 0;
  124.                 for (j = 2; j <= n; j++)
  125.                         if (min > dis[j] && !vis[j])
  126.                         {
  127.                                 min = dis[j];
  128.                                 flag = j;
  129.                         }
  130.                 vis[flag] = 1;
  131.                 for (j = 2; j <= n; j++)
  132.                 {
  133.                         if (dis[j] > min + temp[flag][j] && !vis[j])
  134.                                 dis[j] = min + temp[flag][j];
  135.                 }
  136.         }
  137. }
  138. int main()
  139. {
  140.         int t, i, n, k, m, diss;
  141.         while (cin >> t >> n)
  142.         {
  143.                 //memset(temp,INF,sizeof(temp));
  144.                 for (int i = 1; i <= 2000; i++) temp[i][i] = 0;
  145.                 for (int i = 1; i <= 2000; i++)
  146.                         for (int j = 1; j <= 2000; j++)
  147.                                 temp[i][j] = INF;
  148.                 for (int i = 1; i <= t; i++)
  149.                 {
  150.                         cin >> k >> m >> diss;
  151.                         if (diss < temp[k][m])
  152.                         {
  153.                                 temp[k][m] = diss;//双向导通
  154.                                 temp[m][k] = diss;
  155.                         }
  156.                 }
  157.                 dijkstra(n);
  158.                 //for(int i=1;i<=t;i++) cout<<dis[i]<<endl;
  159.                 cout << dis[n] << endl;
  160.         }
  161.         return 0;
  162. }
  163. #endif

  165. typedef struct {
  166.         float x[2];     /* state: [0]-angle [1]-diffrence of angle, 2x1 */
  167.         float A[2][2];  /* X(n)=A*X(n-1)+U(n),U(n)~N(0,q), 2x2 */
  168.         float H[2][2];     /* Z(n)=H*X(n)+W(n),W(n)~N(0,r), 1x2   */
  169.         float q[2];     /* process(predict) noise convariance,2x1 [q0,0; 0,q1] */
  170.         float r[2][2];        /* measure noise convariance */
  171.         float p[2][2];  /* estimated error convariance,2x2 [p0 p1; p2 p3] */
  172.         float gain[2][2];  /* 2x1 */
  173.         float B[2];
  174. } kalman2_state;

  176. //z轴kalman滤波初始化，初始化时用
  177. // kalman2_init(&BaroAlt_klm);
  178. //输入气压计高度，速度和惯性坐标下的加速度---------输出高度和速度
  179. // kalman2_filter(&BaroAlt_klm, BaroAltoo, 0, az_c);

  181. void kalman2_init(kalman2_state *state)//, float *init_x, float (*init_p)[2]//×îºóÖ»Ðèµ÷ÊÔq[0],q[1];
  182. {
  183.         //    state->x[0]    = init_x[0];
  184.         //    state->x[1]    = init_x[1];
  185.         //    state->p[0][0] = init_p[0][0];
  186.         //    state->p[0][1] = init_p[0][1];
  187.         //    state->p[1][0] = init_p[1][0];
  188.         //    state->p[1][1] = init_p[1][1];
  189.         state->x[0] = 0;
  190.         state->x[1] = 0;
  191.         state->p[0][0] = 1;
  192.         state->p[0][1] = 0;
  193.         state->p[1][0] = 0;
  194.         state->p[1][1] = 1;
  195.         //          state->A       = {{1, 0.1}, {0, 1}};
  196.         state->A[0][0] = 1;
  197.         state->A[0][1] = 0.01;//1;//t¿É±ä  Á½¸öÎïÀíʱ¿ÌµÄ²î2ms      PID_PIT.I*0.27;//0.0027
  198.         state->A[1][0] = 0;
  199.         state->A[1][1] = 1;
  200.         //          state->H       = {1,0};
  201.         state->H[0][0] = 1;
  202.         state->H[0][1] = 0;//1
  203.         state->H[1][0] = 0;
  204.         state->H[1][1] = 0;
  205.         //    state->q       = {{10e-6,0}, {0,10e-6}};  /* measure noise convariance */               
  206.         state->q[0] = 5 * 10e-8;//5;//0.0001;//10e-7;//10e-7;
  207.         state->q[1] = 5 * 10e-8;//10e-6;//0.5;//0.0035;//5*10e-7;
  208.         state->r[0][0] = 1 * 10e-7;//10e-4;//52.4586;//0.1;//10e-3;//10e-7;  /* estimated error convariance */PID_ROL.D*
  209.         state->r[0][1] = 0;
  210.         state->r[1][0] = 0;
  211.         state->r[1][1] = 4 * 10e-4;
  212.         //    state->B               
  213.         state->B[0] = state->A[0][1] * state->A[0][1] / 2.0;
  214.         state->B[1] = state->A[0][1];
  215. }

  217. float kalman2_filter(kalman2_state *state, float x_weiyi, float x_speed, float a)//£¨¶þά¿¨¶ûÂüÂ˲¨£©Ö»±äÒ»¸ö£¬Ðè¸Ä½øλÒÆ¡¢ËÙ¶È,ÕæÕýµÄ¼ÓËٶȣ¨Ð޸ĺó£©
  218. {
  219.         float temp0 = 0.0f;
  220.         float temp1 = 0.0f;
  221.         float temp0_0 = 0.0f;
  222.         float temp0_1 = 0.0f;
  223.         float temp1_0 = 0.0f;
  224.         float temp1_1 = 0.0f;
  225.         float temp00 = 0.0f;
  226.         float temp01 = 0.0f;
  227.         float temp10 = 0.0f;
  228.         float temp11 = 0.0f;

  230.         /* Step1: Predict X(k+1)= A*X(k) +B*U(k)*/
  231.         state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1] + state->B[0] * a;//ת»»Íê×ø±ê·½¿ÉʹÓÃ
  232.         state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1] + state->B[1] * a;
  233.         /* Step2: Covariance Predict P(k+1)=A*P(k)*(A^T)+Q;*/
  234.         state->p[0][0] = (state->p[0][0] + state->p[1][0] * state->A[0][1]) + (state->p[0][1] + state->p[1][1] * state->A[0][1])*state->A[0][1] + state->q[0];
  235.         state->p[0][1] = state->p[0][1] + state->p[1][1] * state->A[0][1];//+state->q[0];
  236.         state->p[1][0] = state->p[1][0] + state->p[1][1] * state->A[0][1];//+state->q[1];
  237.         state->p[1][1] = state->p[1][1] + state->q[1];
  238.         /* Step3: Gain Measurement : gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose.  µÚÈý¸ö¹«Ê½×ª»»*/
  239.         temp0_0 = (state->p[0][0] + state->r[0][0])*(state->p[1][1] + state->r[1][1]) - (state->p[0][1] + state->r[0][1])*(state->p[1][0] + state->r[1][0]);//ÕýÈ·//r¶Ô½ÇÕó
  240.         temp0_1 = (state->p[0][0] + state->r[0][0])*(state->p[1][1] + state->r[1][1]) - (state->p[0][1] + state->r[0][1])*(state->p[1][0] + state->r[1][0]);
  241.         temp1_0 = (state->p[0][0] + state->r[0][0])*(state->p[1][1] + state->r[1][1]) - (state->p[0][1] + state->r[0][1])*(state->p[1][0] + state->r[1][0]);
  242.         temp1_1 = (state->p[0][0] + state->r[0][0])*(state->p[1][1] + state->r[1][1]) - (state->p[0][1] + state->r[0][1])*(state->p[1][0] + state->r[1][0]);
  243.         temp00 = state->p[1][1] / temp0_0;
  244.         temp01 = -state->p[0][1] / temp0_1;
  245.         temp10 = -state->p[1][0] / temp1_0;
  246.         temp11 = state->p[0][0] / temp1_1;
  247.         state->gain[0][0] = state->p[0][0] * temp00 + state->p[0][1] * temp10;
  248.         state->gain[0][1] = state->p[0][0] * temp01 + state->p[0][1] * temp11;
  249.         state->gain[1][0] = state->p[1][0] * temp00 + state->p[1][1] * temp10;
  250.         state->gain[1][1] = state->p[1][0] * temp01 + state->p[1][1] * temp11;
  251.         /* Step4: Status Update : x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/
  252.         state->x[0] = state->x[0] + state->gain[0][0] * (x_weiyi - state->x[0]) + state->gain[0][1] * (x_speed - state->x[1]); //ΪºÎÖ»ÓÃÒ»¸ö²ÎÊý
  253.         state->x[1] = state->x[1] + state->gain[1][0] * (x_weiyi - state->x[0]) + state->gain[1][1] * (x_speed - state->x[1]);
  254.         /* Step5: Covariance Update p: p(n|n) = [I - gain * H] * p(n|n-1)  ¸üÐÂp*/
  255.         temp0 = state->p[0][0];
  256.         temp1 = state->p[0][1];
  257.         state->p[0][0] = (1 - state->gain[0][0]) * state->p[0][0] - (state->gain[0][1] * state->p[1][0]);
  258.         state->p[0][1] = (1 - state->gain[0][0]) * state->p[0][1] - (state->gain[0][1] * state->p[1][1]);
  259.         state->p[1][0] = (1 - state->gain[1][1]) * state->p[1][0] - state->gain[1][0] * temp0;//state->p[0][0]
  260.         state->p[1][1] = (1 - state->gain[1][1]) * state->p[1][1] - state->gain[1][0] * temp1;//state->p[0][1]

  262.         return 1;
  263. }
复制代码
全部资料51hei下载地址：
Dijstra.rar (3 KB, 下载次数: 9)
作者: ahyl    时间: 2019-5-17 11:26
想不到编辑之后、会乱成这个样子



欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1