Dijkstra C++实现

#include "stdafx.h" #include "graph.h" #include <iostream> #include <set> #include <queue> #include <algorithm> #include <math.h> #include <assert.h> #include <time.h> #define maxnum 999999.9 using namespace std; struct grid_graph : graph { grid_graph(int n) { m_n = n; } inline void initial() { Nodes(); } virtual void Nodes() { m_graph.resize(m_n*m_n); for(int i=m_n*m_n; --i>=0;) m_graph[i] = i; } virtual intA Neighbor(int i) { intA nb; int x,y,x1,y1; y = i/m_n; x = i%m_n; int k,id; for(k = -1; k <= 1; k+=2) { x1 = x +k; if((x1 < 0 )|| (x1 >= m_n)) continue; id = y*m_n+x1; nb.push_back(id); } for(k = -1; k <= 1; k+=2) { y1 = y +k; if((y1 < 0) || (y1 >= m_n)) continue; id = y1*m_n+x; nb.push_back(id); } assert(nb.size()>0); return nb; } double W1(int i) { return i*i; } virtual double Weight(int i,int j) { return fabs(W1(i)-W1(j)); } }; double shortpath(graph& G,int start,int end,intA& path) { int i,j,k; int tt = G.m_graph.size(); node tn(0,0,0.0); std::vector<double> w2s(tt,maxnum); std::set<int> M; std::vector<int> S,F,N; std::vector<bool> flag(G.m_n*G.m_n,false); std::priority_queue<node, std::vector<node>, std::greater<node> > Q; S.push_back(start); F.push_back(0); M.insert(start); flag[start] = 1; w2s[start] = 0.0; for(i=0; i<S.size(); ++i) { //flag[i] = true; N.swap( G.Neighbor(S[i]) ); for(j=N.size(); --j>=0;) { k = N[j]; assert(k <= tt); if(flag[k]) continue; w2s[k] = w2s[i] + G.Weight(i,k); Q.push(node(k,i,w2s[k])); if(k==end) break; } if(Q.empty()) break; do{ tn = Q.top(); Q.pop(); }while(flag[tn.m_id]); S.push_back(tn.m_id); F.push_back(tn.m_f_id); flag[tn.m_id] = 1; if(S.back()==end) break; } if(S.back()!=end) return -1; for(i=S.size()-1;i>0;i=F[i]) { path.push_back(S[i]); } path.push_back(start); std::reverse(path.begin(),path.end()); return w2s[end]; } double shortpath1(graph& G,int i,int j,intA& path) { int n = G.m_n*G.m_n; int v,u,k; double maxd = 999999999.9; double mind,temp; std::vector<double> dist(n,maxd); std::vector<int> prev(n,-1); std::vector<bool> visited(n,0); std::vector<int> nb; nb = G.Neighbor(i); for(v = 0; v < nb.size(); v++) { dist[nb[i]] = G.Weight(i,nb[i]); prev[nb[i]] = i; } visited[i]=1; path.push_back(i); for(int cnt = 2; cnt <= n;cnt++) { mind = maxd; for(k = 0; k < n; k++) { if((!visited[k])&& (dist[k] < mind)) { mind = dist[k]; u = k; } } visited[u] = 1; nb = G.Neighbor(u); int id; /*for(k = 0; k < n); k++) { if(!visited[k]]) { temp = dist[u] + G.Weight(u,k); if(dist[k] > temp) { dist[k] = temp; prev[k] = u; } } }*/ for(k = 0; k < nb.size(); k++) { if(!visited[nb[k]]) { temp = dist[u] + G.Weight(u,nb[k]); if(dist[nb[k]] > temp) { dist[nb[k]] = temp; prev[nb[k]] = u; } } } if(visited[j]) break; } u = j; nb.clear(); while(u != i) { nb.push_back(u); u = prev[u]; } for(k = nb.size()-1; k >= 0; k--) { path.push_back(nb[k]); } return dist[j]; } int _tmain(int argc, _TCHAR* argv[]) { int size = 20; grid_graph G(size); G.initial(); intA path; clock_t s,e; s = clock(); double dis = shortpath(G,0,size*size-1,path); //double dis = shortpath1(G,0,size*size-1,path); e = clock(); cout<<"cost time is "<<(e - s)<<" ms \n"; cout<<"path size "<<(int)path.size()<<endl; for(int i = 0; i < path.size(); i++) { std::cout<<path[i]<<"->"; if((i+1)%10 == 0) cout<<endl; } cout<<"\n繚最 "<<dis<<endl; return 0; }