關(guān)于尋求最短路徑的一種算法、附件為算法的C/C++實(shí)現(xiàn),歡迎下載

ID:540551 · 發(fā)表于 2019-5-17 11:25

最短路徑尋優(yōu)

（以下關(guān)于Dijstra的說明，是借用算法與數(shù)據(jù)結(jié)構(gòu)的發(fā)帖說明、侵權(quán)即刪）原帖鏈接最短路徑尋優(yōu)如上圖所示、如何尋求從 A 出發(fā)到 G 點(diǎn)的最短路徑呢？Dijstra算法就是要求出這個最短的路徑；

讓我們來演示一下迪杰斯特拉的詳細(xì)過程：第1步，創(chuàng)建距離表。表中的Key是頂點(diǎn)名稱，Value是從起點(diǎn)A到對應(yīng)頂點(diǎn)的已知最短距離。但是，一開始我們并不知道A到其他頂點(diǎn)的最短距離是多少，Value默認(rèn)是無限大：

第2步，遍歷起點(diǎn)A，找到起點(diǎn)A的鄰接頂點(diǎn)B和C。從A到B的距離是5，從A到C的距離是2。把這一信息刷新到距離表當(dāng)中：

第3步，從距離表中找到從A出發(fā)距離最短的點(diǎn)，也就是頂點(diǎn)C。第4步，遍歷頂點(diǎn)C，找到頂點(diǎn)C的鄰接頂點(diǎn)D和F（A已經(jīng)遍歷過，不需要考慮！！！！！！！！！！代碼編寫中就需要注意這一點(diǎn)）。從C到D的距離是6，所以A到D的距離是2+6=8；從C到F的距離是8，所以從A到F的距離是2+8=10。把這一信息刷新到表中：

接下來重復(fù)第3步、第4步所做的操作：第5步，也就是第3步的重復(fù)，從距離表中找到從A出發(fā)距離最短的點(diǎn)（C已經(jīng)遍歷過，不需要考慮），也就是頂點(diǎn)B。第6步，也就是第4步的重復(fù)，遍歷頂點(diǎn)B，找到頂點(diǎn)B的鄰接頂點(diǎn)D和E（A已經(jīng)遍歷過，不需要考慮）。從B到D的距離是1，所以A到D的距離是5+1=6，小于距離表中的8；從B到E的距離是6，所以從A到E的距離是5+6=11。把這一信息刷新到表中：

（在第6步，A到D的距離從8刷新到6，可以看出距離表所發(fā)揮的作用。距離表通過迭代刷新，用新路徑長度取代舊路徑長度，最終可以得到從起點(diǎn)到其他頂點(diǎn)的最短距離）第7步，從距離表中找到從A出發(fā)距離最短的點(diǎn)（B和C不用考慮），也就是頂點(diǎn)D。第8步，遍歷頂點(diǎn)D，找到頂點(diǎn)D的鄰接頂點(diǎn)E和F。從D到E的距離是1，所以A到E的距離是6+1=7，小于距離表中的11；從D到F的距離是2，所以從A到F的距離是6+2=8，小于距離表中的10。把這一信息刷新到表中：

第9步，從距離表中找到從A出發(fā)距離最短的點(diǎn)，也就是頂點(diǎn)E。第10步，遍歷頂點(diǎn)E，找到頂點(diǎn)E的鄰接頂點(diǎn)G。從E到G的距離是7，所以A到G的距離是7+7=14。把這一信息刷新到表中：

第11步，從距離表中找到從A出發(fā)距離最短的點(diǎn)，也就是頂點(diǎn)F。第10步，遍歷頂點(diǎn)F，找到頂點(diǎn)F的鄰接頂點(diǎn)G。從F到G的距離是3，所以A到G的距離是8+3=11，小于距離表中的14。把這一信息刷新到表中：

就這樣，除終點(diǎn)以外的全部頂點(diǎn)都已經(jīng)遍歷完畢，距離表中存儲的是從起點(diǎn)A到所有頂點(diǎn)的最短距離。顯然，從A到G的最短距離是11。（路徑：A-B-D-F-G）
下面附上算法的C++實(shí)現(xiàn)

// dijstra.cpp : 此文件包含 "main" 函數(shù)。程序執(zhí)行將在此處開始并結(jié)束。
//
//利用狀態(tài)機(jī)來描述 dijstra算法
//尋求最短路徑
#include "pch.h"
#include <iostream>
using namespace std;
typedef struct {
char nextPointName;
int distance;
}NEXT_POINT;
typedef struct {
int curVaule;
int expandFlag;
char name;
int linkNum;
NEXT_POINT nextPoint[5];
char route[10] = {'A'};
}POINT;
POINT A = { 1000,0,'A' ,2,'B',5,'C',2};
POINT B = { 1000,0,'B' ,2,'D',1,'E',6};
POINT C = { 1000,0,'C' ,2,'D',6,'F',8};
POINT D = { 1000,0,'D' ,2,'E',1,'F',2};
POINT E = { 1000,0,'E' ,1,'G',7};
POINT F = { 1000,0,'F' ,1,'G',3};
POINT G = { 1000,0,'G' };
void Dijkstra(POINT* startPoint, POINT* endPoint, POINT* piontArray, int pointNum);
int main()
{
POINT array[10] = { A,B,C,D,E,F,G };
Dijkstra(&A,&G,array,7);
cout << " 最短路徑為 "<<G.route <<endl<<" 最短路徑長度為 "<< G.curVaule << endl;
system("pause");
}
void Dijkstra(POINT* startPoint, POINT* endPoint, POINT* piontArray,int pointNum) {
POINT unexpandPoint[20] = {0};
int leftNum = pointNum;
POINT expandPoint=*(startPoint);
int temp = 0;
for (int i = 0; i < pointNum; ++i) {
unexpandPoint[i] = piontArray[i];
}
unexpandPoint[0].curVaule = 0;
while (1) {
expandPoint = unexpandPoint[0];
for (int i = 0; i < leftNum; ++i) {
if (expandPoint.curVaule > unexpandPoint[i].curVaule) {
expandPoint = unexpandPoint[i];
temp = i;
}
}
if (expandPoint.name == endPoint->name) {
*(endPoint) = expandPoint;
break;
}
for (int i = 0; i < leftNum; ++i) {
if (i > temp) {
unexpandPoint[i - 1] = unexpandPoint[i];
}
}
temp = 0;
leftNum--;
for (int i = 0; i < expandPoint.linkNum; ++i) {
for (int j = 0; j < leftNum; ++j) {
if (expandPoint.nextPoint[i].nextPointName == unexpandPoint[j].name) {
if (unexpandPoint[j].curVaule > expandPoint.nextPoint[i].distance + expandPoint.curVaule) {
unexpandPoint[j].curVaule = expandPoint.nextPoint[i].distance+ expandPoint.curVaule;
}
if (unexpandPoint[j].curVaule > 1000) {
unexpandPoint[j].curVaule -= 1000;
}
//路徑更新
for (int k = 0; k < 10; ++k) {
unexpandPoint[j].route[k] = 0;
}
for (int k = 0; k < 10; ++k) {
if (expandPoint.route[k] != 0) {
unexpandPoint[j].route[k] = expandPoint.route[k];
}
else {
unexpandPoint[j].route[k] = unexpandPoint[j].name;
break;
}
}
}
}
}
}
}
#if 0
##網(wǎng)上查找到的關(guān)于 dajstra 算法的較精簡的代碼例程
#include<iostream>
#include<cstring>
#define INF 10000000
using namespace std;
int temp[2005][2005], dis[1005];
void dijkstra(int n)
{
int i, min, flag, j, vis[2005] = { 0 };
for (i = 1; i <= n; i++) dis[i] = temp[1][i];
for (i = 1; i < n; i++)
{
min = INF;
flag = 0;
for (j = 2; j <= n; j++)
if (min > dis[j] && !vis[j])
{
min = dis[j];
flag = j;
}
vis[flag] = 1;
for (j = 2; j <= n; j++)
{
if (dis[j] > min + temp[flag][j] && !vis[j])
dis[j] = min + temp[flag][j];
}
}
}
int main()
{
int t, i, n, k, m, diss;
while (cin >> t >> n)
{
//memset(temp,INF,sizeof(temp));
for (int i = 1; i <= 2000; i++) temp[i][i] = 0;
for (int i = 1; i <= 2000; i++)
for (int j = 1; j <= 2000; j++)
temp[i][j] = INF;
for (int i = 1; i <= t; i++)
{
cin >> k >> m >> diss;
if (diss < temp[k][m])
{
temp[k][m] = diss;//雙向?qū)?
temp[m][k] = diss;
}
}
dijkstra(n);
//for(int i=1;i<=t;i++) cout<<dis[i]<<endl;
cout << dis[n] << endl;
}
return 0;
}
#endif
typedef struct {
float x[2]; /* state: [0]-angle [1]-diffrence of angle, 2x1 */
float A[2][2]; /* X(n)=A*X(n-1)+U(n),U(n)~N(0,q), 2x2 */
float H[2][2]; /* Z(n)=H*X(n)+W(n),W(n)~N(0,r), 1x2 */
float q[2]; /* process(predict) noise convariance,2x1 [q0,0; 0,q1] */
float r[2][2]; /* measure noise convariance */
float p[2][2]; /* estimated error convariance,2x2 [p0 p1; p2 p3] */
float gain[2][2]; /* 2x1 */
float B[2];
} kalman2_state;
//z軸kalman濾波初始化，初始化時用
// kalman2_init(&BaroAlt_klm);
//輸入氣壓計高度，速度和慣性坐標(biāo)下的加速度---------輸出高度和速度
// kalman2_filter(&BaroAlt_klm, BaroAltoo, 0, az_c);
void kalman2_init(kalman2_state *state)//, float *init_x, float (*init_p)[2]//×îºóÖ»Ðèµ÷ÊÔq[0],q[1];
{
// state->x[0] = init_x[0];
// state->x[1] = init_x[1];
// state->p[0][0] = init_p[0][0];
// state->p[0][1] = init_p[0][1];
// state->p[1][0] = init_p[1][0];
// state->p[1][1] = init_p[1][1];
state->x[0] = 0;
state->x[1] = 0;
state->p[0][0] = 1;
state->p[0][1] = 0;
state->p[1][0] = 0;
state->p[1][1] = 1;
// state->A = {{1, 0.1}, {0, 1}};
state->A[0][0] = 1;
state->A[0][1] = 0.01;//1;//t¿É±ä Á½¸öÎïÀíÊ±¿ÌµÄ²î2ms PID_PIT.I*0.27;//0.0027
state->A[1][0] = 0;
state->A[1][1] = 1;
// state->H = {1,0};
state->H[0][0] = 1;
state->H[0][1] = 0;//1
state->H[1][0] = 0;
state->H[1][1] = 0;
// state->q = {{10e-6,0}, {0,10e-6}}; /* measure noise convariance */
state->q[0] = 5 * 10e-8;//5;//0.0001;//10e-7;//10e-7;
state->q[1] = 5 * 10e-8;//10e-6;//0.5;//0.0035;//5*10e-7;
state->r[0][0] = 1 * 10e-7;//10e-4;//52.4586;//0.1;//10e-3;//10e-7; /* estimated error convariance */PID_ROL.D*
state->r[0][1] = 0;
state->r[1][0] = 0;
state->r[1][1] = 4 * 10e-4;
// state->B
state->B[0] = state->A[0][1] * state->A[0][1] / 2.0;
state->B[1] = state->A[0][1];
}
float kalman2_filter(kalman2_state *state, float x_weiyi, float x_speed, float a)//£¨¶þÎ¬¿¨¶ûÂüÂË²¨£©Ö»±äÒ»¸ö£¬Ðè¸Ä½øÎ»ÒÆ¡¢ËÙ¶È,ÕæÕýµÄ¼ÓËÙ¶È£¨ÐÞ¸Äºó£©
{
float temp0 = 0.0f;
float temp1 = 0.0f;
float temp0_0 = 0.0f;
float temp0_1 = 0.0f;
float temp1_0 = 0.0f;
float temp1_1 = 0.0f;
float temp00 = 0.0f;
float temp01 = 0.0f;
float temp10 = 0.0f;
float temp11 = 0.0f;
/* Step1: Predict X(k+1)= A*X(k) +B*U(k)*/
state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1] + state->B[0] * a;//×ª»»Íê×ø±ê·½¿ÉÊ¹ÓÃ
state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1] + state->B[1] * a;
/* Step2: Covariance Predict P(k+1)=A*P(k)*(A^T)+Q;*/
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];
state->p[0][1] = state->p[0][1] + state->p[1][1] * state->A[0][1];//+state->q[0];
state->p[1][0] = state->p[1][0] + state->p[1][1] * state->A[0][1];//+state->q[1];
state->p[1][1] = state->p[1][1] + state->q[1];
/* Step3: Gain Measurement : gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose. µÚÈý¸ö¹«Ê½×ª»»*/
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¶Ô½ÇÕó
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]);
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]);
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]);
temp00 = state->p[1][1] / temp0_0;
temp01 = -state->p[0][1] / temp0_1;
temp10 = -state->p[1][0] / temp1_0;
temp11 = state->p[0][0] / temp1_1;
state->gain[0][0] = state->p[0][0] * temp00 + state->p[0][1] * temp10;
state->gain[0][1] = state->p[0][0] * temp01 + state->p[0][1] * temp11;
state->gain[1][0] = state->p[1][0] * temp00 + state->p[1][1] * temp10;
state->gain[1][1] = state->p[1][0] * temp01 + state->p[1][1] * temp11;
/* Step4: Status Update : x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/
state->x[0] = state->x[0] + state->gain[0][0] * (x_weiyi - state->x[0]) + state->gain[0][1] * (x_speed - state->x[1]); //ÎªºÎÖ»ÓÃÒ»¸ö²ÎÊý
state->x[1] = state->x[1] + state->gain[1][0] * (x_weiyi - state->x[0]) + state->gain[1][1] * (x_speed - state->x[1]);
/* Step5: Covariance Update p: p(n|n) = [I - gain * H] * p(n|n-1) ¸üÐÂp*/
temp0 = state->p[0][0];
temp1 = state->p[0][1];
state->p[0][0] = (1 - state->gain[0][0]) * state->p[0][0] - (state->gain[0][1] * state->p[1][0]);
state->p[0][1] = (1 - state->gain[0][0]) * state->p[0][1] - (state->gain[0][1] * state->p[1][1]);
state->p[1][0] = (1 - state->gain[1][1]) * state->p[1][0] - state->gain[1][0] * temp0;//state->p[0][0]
state->p[1][1] = (1 - state->gain[1][1]) * state->p[1][1] - state->gain[1][0] * temp1;//state->p[0][1]
return 1;
}

復(fù)制代碼

全部資料51hei下載地址：

Dijstra.rar (3 KB, 下載次數(shù): 9)

ID:540551 · 發(fā)表于 2019-5-17 11:26

想不到編輯之后、會亂成這個樣子

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

關(guān)于尋求最短路徑的一種算法、附件為算法的C/C++實(shí)現(xiàn),歡迎下載

評分