Shortest path algorithm

[totototo]

Hi,
I am trying to write a code to compute the shortest path between any two nodes in a graph. I wrote the code but the problem is that it is very slow when I run it for let us say 65 nodes (for every pair of nodes, the algorithm computes the shortest path). I saw on the internet that there are ways to do so, but they are very complicated codes. However, in my code, I use the Priority Queue built-in package so that I quickly extract the smallest value. Could you please help me in making the code fast?

An example of a graph:

1--(distance = 9)---> 2 ---(distance = 12)------> 3
| ^
| (distance = 10) | (distance=1)
V |
4 --------(distance = 2)------------------------------> 5

The path from node 1 to node 3: 1,4,5,3 is the shortest because 13 < 21

[CODE=C++]
double Spanner::Find_distance_UDG(int start_vertex,int end_vertex)
{
Thread a;// Just assume that you only need two things from this class index(the label of a node), and dist (the distance between a node and its neighbor)
priority_queue QQ1;
queue Q1;
int UNVISITED = 0,VISITED=2;//,UNDEFINED = -1;
init(); //it makes all the nodes unvisited
int v, w;
double distance = 0;
double final_distance = 100000000;
double power_distance = 0; // ignore this variable whenever you find it in the code
a.index = start_vertex;
a.dist = distance;
a.dist_power = 0.0; // ignore this variable whenever you find it in the code
QQ1.push(a);
setMark(start_vertex, VISITED); //mark this node as a visited node
while (QQ1.size()!= 0)
{
v=QQ1.top().index;
distance = QQ1.top().dist;
power_distance = QQ1.top().dist_power;
QQ1.pop();

if ( v == end_vertex )
{
final_distance = distance;
least_power = power_distance;
return final_distance;
}
else
{
for (int k = 0; k < nodes[v].nbrcnt2; k++ )
{
w = nodes[v].nbrlist2[k]; //Each node v has some neighbors in its list nbrlist2
if (getMark(w) == UNVISITED) //To see if this node has not been visited before
{
setMark(w, VISITED); // Change the node status to visited
a.index = w;
a.dist = distance+ dis(v,w);
a.dist_power = power_distance + (pow(dis(v,w),2)*1.000000);
QQ1.push(a);

}
else
{
int size = QQ1.size();
int flag = 0; //This flag is used to see if the node is already in the priority queue
while(size !=0 && flag == 0)
{
Thread b;
b.index = QQ1.top().index;
b.dist = QQ1.top().dist;
b.dist_power = QQ1.top().dist_power;
QQ1.pop();
if(b.index == w)
{ flag = 1;
if(b.dist > (distance + dis(v,w)))
{ b.dist = (distance + dis(v,w));
b.dist_power = power_distance + (pow(dis(v,w),2)*1.000000);
}
}
size--;
Q1.push(b);

}
while(Q1.size()!=0)
{
Thread b;
b.index = Q1.front().index;
b.dist = Q1.front().dist;
b.dist_power = Q1.front().dist_power;
Q1.pop();
QQ1.push(b);
}
}

}//for (int k = 0; k < nodes[v].nbrcnt2; k++ )

}//else



}

if( final_distance == 100000000) //This is just for testing purposes, however it did not reach it before
final_distance = 0;
return final_distance;


}

[/code]