| 1 |
francois |
1156 |
//####//------------------------------------------------------------ |
| 2 |
|
|
//####//------------------------------------------------------------ |
| 3 |
|
|
//####// MAGiC |
| 4 |
|
|
//####// Jean Christophe Cuilliere et Vincent FRANCOIS |
| 5 |
|
|
//####// Departement de Genie Mecanique - UQTR |
| 6 |
|
|
//####//------------------------------------------------------------ |
| 7 |
|
|
//####// MAGIC est un projet de recherche de l equipe ERICCA |
| 8 |
|
|
//####// du departement de genie mecanique de l Universite du Quebec a Trois Rivieres |
| 9 |
|
|
//####// http://www.uqtr.ca/ericca |
| 10 |
|
|
//####// http://www.uqtr.ca/ |
| 11 |
|
|
//####//------------------------------------------------------------ |
| 12 |
|
|
//####//------------------------------------------------------------ |
| 13 |
|
|
//####// |
| 14 |
|
|
//####// hypergraphlib_dijkstra.cpp |
| 15 |
|
|
//####// |
| 16 |
|
|
//####//------------------------------------------------------------ |
| 17 |
|
|
//####//------------------------------------------------------------ |
| 18 |
|
|
//####// COPYRIGHT 2000-2024 |
| 19 |
|
|
//####// jeu 13 jun 2024 11:54:00 EDT |
| 20 |
|
|
//####//------------------------------------------------------------ |
| 21 |
|
|
//####//------------------------------------------------------------ |
| 22 |
francois |
481 |
#include "hypergraphlib_dijkstra.h"
|
| 23 |
francois |
102 |
|
| 24 |
francois |
481 |
#include "hypergraphlib_graph.h"
|
| 25 |
|
|
#include "hypergraphlib_node.h"
|
| 26 |
|
|
#include "hypergraphlib_arc.h"
|
| 27 |
|
|
|
| 28 |
|
|
#include <stdio.h>
|
| 29 |
|
|
|
| 30 |
|
|
using namespace HypergraphLib;
|
| 31 |
|
|
|
| 32 |
|
|
struct DJNode {
|
| 33 |
|
|
int nbVisit;
|
| 34 |
|
|
double d;
|
| 35 |
|
|
Node * n;
|
| 36 |
|
|
Node * previous;
|
| 37 |
|
|
Arc * prevArc;
|
| 38 |
|
|
};
|
| 39 |
|
|
|
| 40 |
|
|
double
|
| 41 |
|
|
HypergraphLib::Dijkstra(Graph * __G, Node * source, Node * destination, double (* distanceFunc) (HypergraphLib::Node*, HypergraphLib::Node*, HypergraphLib::Arc*) , std::vector<Node *> & path, std::vector<Arc*> & pathArcs)
|
| 42 |
|
|
{
|
| 43 |
|
|
int i,j,k;
|
| 44 |
|
|
|
| 45 |
|
|
#define djnode(xtmp) ((DJNode*)xtmp->GetUserData(50))
|
| 46 |
|
|
|
| 47 |
|
|
|
| 48 |
|
|
Graph::MapNodesById graphNodes = __G->GetNodes();
|
| 49 |
|
|
for (Graph::MapNodesById::iterator itNode = graphNodes.begin();
|
| 50 |
|
|
itNode != graphNodes.end();
|
| 51 |
|
|
itNode ++ )
|
| 52 |
|
|
{
|
| 53 |
|
|
Node * n = itNode->second;
|
| 54 |
|
|
struct DJNode * dj = new struct DJNode;
|
| 55 |
|
|
dj->previous = NULL;
|
| 56 |
|
|
dj->n = n;
|
| 57 |
|
|
dj->nbVisit = 0;
|
| 58 |
|
|
dj->d = 1E300;
|
| 59 |
|
|
dj->prevArc = 0;
|
| 60 |
|
|
|
| 61 |
|
|
n->SetUserData(50,dj);
|
| 62 |
|
|
}
|
| 63 |
|
|
|
| 64 |
|
|
if (__G->GetNode(source->Id()) != source || __G->GetNode(destination->Id()) != destination )
|
| 65 |
|
|
return 1E300;
|
| 66 |
|
|
|
| 67 |
|
|
djnode(source)->d = 0;
|
| 68 |
|
|
|
| 69 |
|
|
typedef std::pair < double, DJNode * > DJElement;
|
| 70 |
|
|
std::set < DJElement > Q;
|
| 71 |
|
|
std::set < DJElement >::iterator itQ;
|
| 72 |
|
|
DJNode* djSource=djnode(source);
|
| 73 |
|
|
DJNode* djDestination=djnode(destination);
|
| 74 |
|
|
Q.insert(DJElement(djSource->d,djSource));
|
| 75 |
|
|
|
| 76 |
|
|
while ( ! Q.empty() )
|
| 77 |
|
|
{
|
| 78 |
|
|
struct DJNode * current = NULL;
|
| 79 |
|
|
|
| 80 |
|
|
DJElement top = *Q.begin();
|
| 81 |
|
|
Q.erase(Q.begin());
|
| 82 |
|
|
current = top.second;
|
| 83 |
|
|
|
| 84 |
|
|
if ( current == NULL )
|
| 85 |
|
|
break;
|
| 86 |
|
|
|
| 87 |
|
|
current->nbVisit++;
|
| 88 |
|
|
|
| 89 |
|
|
for ( Node::MultimapArcsById::iterator itArc1 = current->n->IncidentArcs().begin();
|
| 90 |
|
|
itArc1 != current->n->IncidentArcs().end();
|
| 91 |
|
|
itArc1 ++ )
|
| 92 |
|
|
{
|
| 93 |
|
|
Arc * arc = itArc1->second;
|
| 94 |
|
|
for ( Arc::MultimapNodesById::iterator itNode = arc->Nodes().begin();
|
| 95 |
|
|
itNode != arc->Nodes().end();
|
| 96 |
|
|
itNode ++ )
|
| 97 |
|
|
{
|
| 98 |
|
|
Node * child = itNode->second;
|
| 99 |
|
|
if (child == current->n)
|
| 100 |
|
|
continue;
|
| 101 |
|
|
|
| 102 |
|
|
struct DJNode * sDJNodeChild = djnode(child);
|
| 103 |
|
|
double dist_child = distanceFunc(child, current->n, arc);
|
| 104 |
|
|
|
| 105 |
|
|
if ( sDJNodeChild->d > current->d + dist_child)
|
| 106 |
|
|
{
|
| 107 |
|
|
if (sDJNodeChild->d < 1E100) {
|
| 108 |
|
|
itQ = Q.find(DJElement(sDJNodeChild->d,sDJNodeChild));
|
| 109 |
|
|
if (itQ != Q.end())
|
| 110 |
|
|
Q.erase(itQ);
|
| 111 |
|
|
}
|
| 112 |
|
|
|
| 113 |
|
|
sDJNodeChild->d = current->d + dist_child;
|
| 114 |
|
|
sDJNodeChild->previous = current->n;
|
| 115 |
|
|
sDJNodeChild->prevArc = arc;
|
| 116 |
|
|
|
| 117 |
|
|
Q.insert(DJElement(sDJNodeChild->d,sDJNodeChild));
|
| 118 |
|
|
}
|
| 119 |
|
|
}
|
| 120 |
|
|
}
|
| 121 |
|
|
}
|
| 122 |
|
|
|
| 123 |
|
|
struct DJNode * n = djDestination;
|
| 124 |
|
|
double length = n->d;
|
| 125 |
|
|
|
| 126 |
|
|
int nb_node = __G->GetNodes().size();
|
| 127 |
|
|
|
| 128 |
|
|
for (i=0; i<nb_node ; i++)
|
| 129 |
|
|
{
|
| 130 |
|
|
path.insert(path.begin(), n->n);
|
| 131 |
|
|
if (n->prevArc)
|
| 132 |
|
|
pathArcs.insert(pathArcs.begin(), n->prevArc);
|
| 133 |
|
|
if ( n->n == source )
|
| 134 |
|
|
break;
|
| 135 |
|
|
else
|
| 136 |
|
|
{
|
| 137 |
|
|
if (n->previous)
|
| 138 |
|
|
n = djnode(n->previous);
|
| 139 |
|
|
else
|
| 140 |
|
|
break;
|
| 141 |
|
|
}
|
| 142 |
|
|
}
|
| 143 |
|
|
if (i == nb_node)
|
| 144 |
|
|
length = 1E300;
|
| 145 |
|
|
|
| 146 |
|
|
for (Graph::MapNodesById::iterator itNode = graphNodes.begin();
|
| 147 |
|
|
itNode != graphNodes.end();
|
| 148 |
|
|
itNode ++ )
|
| 149 |
|
|
{
|
| 150 |
|
|
Node * node = itNode->second;
|
| 151 |
|
|
struct DJNode * djtmp = djnode(node);
|
| 152 |
|
|
delete djtmp;
|
| 153 |
|
|
node->SetUserData(50,0);
|
| 154 |
|
|
}
|
| 155 |
|
|
#undef djnode
|
| 156 |
|
|
|
| 157 |
|
|
return length;
|
| 158 |
|
|
}
|
| 159 |
|
|
|
| 160 |
|
|
|
