hypergraphlib_dijkstra.cpp

Aller à la documentation de ce fichier.

//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//  MAGiC
//####//  Jean Christophe Cuilliere et Vincent FRANCOIS
//####//  Departement de Genie Mecanique - UQTR
//####//------------------------------------------------------------
//####//  MAGIC est un projet de recherche de l equipe ERICCA
//####//  du departement de genie mecanique de l Universite du Quebec a Trois Rivieres
//####//  http://www.uqtr.ca/ericca
//####//  http://www.uqtr.ca/
//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//
//####//       hypergraphlib_dijkstra.cpp
//####//
//####//------------------------------------------------------------
//####//------------------------------------------------------------
//####//    COPYRIGHT 2000-2024
//####//  jeu 13 jun 2024 11:54:00 EDT
//####//------------------------------------------------------------
//####//------------------------------------------------------------
#include "hypergraphlib_dijkstra.h"
 
#include "hypergraphlib_graph.h"
#include "hypergraphlib_node.h"
#include "hypergraphlib_arc.h"
 
#include <stdio.h>
 
using namespace HypergraphLib;
 
struct DJNode {
    int nbVisit;
    double d;
    Node * n;
    Node * previous;
    Arc * prevArc;
};
 
double
HypergraphLib::Dijkstra(Graph * __G, Node * source, Node * destination, double (* distanceFunc)  (HypergraphLib::Node*, HypergraphLib::Node*, HypergraphLib::Arc*) , std::vector<Node *> & path, std::vector<Arc*> & pathArcs)
{
    int i,j,k;
 
#define djnode(xtmp) ((DJNode*)xtmp->GetUserData(50))
 
 
    Graph::MapNodesById graphNodes = __G->GetNodes();
    for (Graph::MapNodesById::iterator itNode = graphNodes.begin();
            itNode != graphNodes.end();
            itNode ++ )
    {
        Node * n = itNode->second;
        struct DJNode * dj = new struct DJNode;
        dj->previous = NULL;
        dj->n = n;
        dj->nbVisit = 0;
        dj->d = 1E300;
        dj->prevArc = 0;
 
        n->SetUserData(50,dj);
    }
 
    if (__G->GetNode(source->Id()) != source || __G->GetNode(destination->Id()) != destination )
        return 1E300;
 
    djnode(source)->d = 0;
 
    typedef std::pair < double, DJNode * > DJElement;
    std::set < DJElement > Q;
    std::set < DJElement >::iterator itQ;
    DJNode* djSource=djnode(source);
    DJNode* djDestination=djnode(destination);
    Q.insert(DJElement(djSource->d,djSource));
 
    while ( ! Q.empty() )
    {
        struct DJNode * current = NULL;
 
        DJElement top = *Q.begin();
        Q.erase(Q.begin());
        current = top.second;
 
        if ( current == NULL )
            break;
 
        current->nbVisit++;
 
        for ( Node::MultimapArcsById::iterator itArc1 = current->n->IncidentArcs().begin();
                itArc1 != current->n->IncidentArcs().end();
                itArc1 ++    )
        {
            Arc * arc = itArc1->second;
            for ( Arc::MultimapNodesById::iterator itNode = arc->Nodes().begin();
                    itNode != arc->Nodes().end();
                    itNode ++    )
            {
                Node * child = itNode->second;
                if (child == current->n)
                    continue;
 
                struct DJNode * sDJNodeChild = djnode(child);
                double dist_child = distanceFunc(child, current->n, arc);
 
                if ( sDJNodeChild->d > current->d + dist_child)
                {
                    if (sDJNodeChild->d < 1E100) {
                        itQ =  Q.find(DJElement(sDJNodeChild->d,sDJNodeChild));
                        if (itQ != Q.end())
                            Q.erase(itQ);
                    }
 
                    sDJNodeChild->d = current->d + dist_child;
                    sDJNodeChild->previous = current->n;
                    sDJNodeChild->prevArc = arc;
 
                    Q.insert(DJElement(sDJNodeChild->d,sDJNodeChild));
                }
            }
        }
    }
 
    struct DJNode * n = djDestination;
    double length = n->d;
 
    int nb_node = __G->GetNodes().size();
 
    for (i=0; i<nb_node ; i++)
    {
        path.insert(path.begin(), n->n);
        if (n->prevArc)
            pathArcs.insert(pathArcs.begin(), n->prevArc);
        if ( n->n == source )
            break;
        else
        {
            if (n->previous)
                n = djnode(n->previous);
            else
                break;
        }
    }
    if (i == nb_node)
        length = 1E300;
 
    for (Graph::MapNodesById::iterator itNode = graphNodes.begin();
            itNode != graphNodes.end();
            itNode ++ )
    {
        Node * node = itNode->second;
        struct DJNode * djtmp = djnode(node);
        delete djtmp;
        node->SetUserData(50,0);
    }
#undef djnode
 
    return length;
}