| 1 |
//####//------------------------------------------------------------ |
| 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 |
#include "hypergraphlib_dijkstra.h"
|
| 23 |
|
| 24 |
#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 |
|
