| 1 |
foucault |
27 |
#include "HypergraphLib_Dijkstra.h"
|
| 2 |
|
|
|
| 3 |
|
|
#include "HypergraphLib_Graph.h"
|
| 4 |
|
|
#include "HypergraphLib_Node.h"
|
| 5 |
|
|
#include "HypergraphLib_Arc.h"
|
| 6 |
|
|
|
| 7 |
|
|
#include <stdio.h>
|
| 8 |
|
|
|
| 9 |
|
|
using namespace HypergraphLib;
|
| 10 |
|
|
|
| 11 |
|
|
//---------------------------------------------------------------------------
|
| 12 |
|
|
struct DJNode {
|
| 13 |
|
|
int nbVisit;
|
| 14 |
|
|
double d;
|
| 15 |
|
|
Node * n;
|
| 16 |
|
|
Node * previous;
|
| 17 |
|
|
Arc * prevArc;
|
| 18 |
|
|
};
|
| 19 |
|
|
|
| 20 |
|
|
//---------------------------------------------------------------------------
|
| 21 |
|
|
// Calcule le chemin le plus court entre les noeuds a et b
|
| 22 |
|
|
double
|
| 23 |
|
|
HypergraphLib::Dijkstra(Graph * __G, Node * source, Node * destination, double (* distanceFunc) (HypergraphLib::Node*, HypergraphLib::Node*, HypergraphLib::Arc*) , std::vector<Node *> & path, std::vector<Arc*> & pathArcs)
|
| 24 |
|
|
{
|
| 25 |
|
|
int i,j,k;
|
| 26 |
|
|
|
| 27 |
|
|
#define djnode(xtmp) ((DJNode*)xtmp->GetUserData(50))
|
| 28 |
|
|
|
| 29 |
|
|
|
| 30 |
|
|
Graph::MapNodesById graphNodes = __G->GetNodes();
|
| 31 |
|
|
for (Graph::MapNodesById::iterator itNode = graphNodes.begin();
|
| 32 |
|
|
itNode != graphNodes.end();
|
| 33 |
|
|
itNode ++ )
|
| 34 |
|
|
{
|
| 35 |
|
|
Node * n = itNode->second;
|
| 36 |
|
|
struct DJNode * dj = new struct DJNode;
|
| 37 |
|
|
dj->previous = NULL;
|
| 38 |
|
|
dj->n = n;
|
| 39 |
|
|
dj->nbVisit = 0;
|
| 40 |
|
|
dj->d = 1E300;
|
| 41 |
|
|
dj->prevArc = 0;
|
| 42 |
|
|
|
| 43 |
|
|
n->SetUserData(50,dj);
|
| 44 |
|
|
}
|
| 45 |
|
|
|
| 46 |
|
|
if (__G->GetNode(source->Id()) != source || __G->GetNode(destination->Id()) != destination )
|
| 47 |
|
|
return 1E300;
|
| 48 |
|
|
|
| 49 |
|
|
djnode(source)->d = 0;
|
| 50 |
|
|
|
| 51 |
|
|
typedef std::pair < double, DJNode * > DJElement;
|
| 52 |
|
|
std::set < DJElement > Q;
|
| 53 |
|
|
DJNode* djSource=djnode(source);
|
| 54 |
|
|
DJNode* djDestination=djnode(destination);
|
| 55 |
|
|
Q.insert(DJElement(djSource->d,djSource));
|
| 56 |
|
|
|
| 57 |
|
|
while ( ! Q.empty() )
|
| 58 |
|
|
{
|
| 59 |
|
|
struct DJNode * current = NULL;
|
| 60 |
|
|
|
| 61 |
|
|
// again, fetch the closest to start element
|
| 62 |
|
|
// from “queue” organized via set
|
| 63 |
|
|
DJElement top = *Q.begin();
|
| 64 |
|
|
Q.erase(Q.begin());
|
| 65 |
|
|
current = top.second;
|
| 66 |
|
|
|
| 67 |
|
|
if ( current == NULL )
|
| 68 |
|
|
break;
|
| 69 |
|
|
|
| 70 |
|
|
current->nbVisit++;
|
| 71 |
|
|
|
| 72 |
|
|
for ( Node::MultimapArcsById::iterator itArc1 = current->n->IncidentArcs().begin();
|
| 73 |
|
|
itArc1 != current->n->IncidentArcs().end();
|
| 74 |
|
|
itArc1 ++ )
|
| 75 |
|
|
{
|
| 76 |
|
|
Arc * arc = itArc1->second;
|
| 77 |
|
|
for ( Arc::MultimapNodesById::iterator itNode = arc->Nodes().begin();
|
| 78 |
|
|
itNode != arc->Nodes().end();
|
| 79 |
|
|
itNode ++ )
|
| 80 |
|
|
{
|
| 81 |
|
|
Node * child = itNode->second;
|
| 82 |
|
|
if (child == current->n)
|
| 83 |
|
|
continue;
|
| 84 |
|
|
|
| 85 |
|
|
struct DJNode * sDJNodeChild = djnode(child);//&(nodes[child]);
|
| 86 |
|
|
double dist_child = distanceFunc(child, current->n, arc);
|
| 87 |
|
|
|
| 88 |
|
|
if ( sDJNodeChild->d > current->d + dist_child)
|
| 89 |
|
|
{
|
| 90 |
|
|
if(sDJNodeChild->d < 1E100) {
|
| 91 |
|
|
Q.erase(Q.find(DJElement(sDJNodeChild->d,sDJNodeChild)));
|
| 92 |
|
|
}
|
| 93 |
|
|
|
| 94 |
|
|
sDJNodeChild->d = current->d + dist_child;
|
| 95 |
|
|
sDJNodeChild->previous = current->n;
|
| 96 |
|
|
sDJNodeChild->prevArc = arc;
|
| 97 |
|
|
|
| 98 |
|
|
Q.insert(DJElement(sDJNodeChild->d,sDJNodeChild));
|
| 99 |
|
|
}
|
| 100 |
|
|
}
|
| 101 |
|
|
}
|
| 102 |
|
|
}
|
| 103 |
|
|
|
| 104 |
|
|
struct DJNode * n = djDestination;
|
| 105 |
|
|
double length = n->d;
|
| 106 |
|
|
|
| 107 |
|
|
int nb_node = __G->GetNodes().size();
|
| 108 |
|
|
|
| 109 |
|
|
for (i=0; i<nb_node ; i++)
|
| 110 |
|
|
{
|
| 111 |
|
|
/*printf("dist=%f", n->d);
|
| 112 |
|
|
if (n->path)
|
| 113 |
|
|
printf("arc=%d\n", n->path->Id()); */
|
| 114 |
|
|
path.insert(path.begin(), n->n);
|
| 115 |
|
|
if (n->prevArc)
|
| 116 |
|
|
pathArcs.insert(pathArcs.begin(), n->prevArc);
|
| 117 |
|
|
if ( n->n == source )
|
| 118 |
|
|
break;
|
| 119 |
|
|
else
|
| 120 |
|
|
{
|
| 121 |
|
|
if (n->previous)
|
| 122 |
|
|
n = djnode(n->previous);
|
| 123 |
|
|
else
|
| 124 |
|
|
break;
|
| 125 |
|
|
}
|
| 126 |
|
|
}
|
| 127 |
|
|
if (i == nb_node)
|
| 128 |
|
|
length = 1E300;
|
| 129 |
|
|
|
| 130 |
|
|
for (Graph::MapNodesById::iterator itNode = graphNodes.begin();
|
| 131 |
|
|
itNode != graphNodes.end();
|
| 132 |
|
|
itNode ++ )
|
| 133 |
|
|
{
|
| 134 |
|
|
Node * node = itNode->second;
|
| 135 |
|
|
struct DJNode * djtmp = djnode(node);
|
| 136 |
|
|
delete djtmp;
|
| 137 |
|
|
node->SetUserData(50,0);
|
| 138 |
|
|
}
|
| 139 |
|
|
#undef djnode
|
| 140 |
|
|
|
| 141 |
|
|
return length;
|
| 142 |
|
|
}
|
