[c] Algoritmo di Dijkstra per la ricerca dei percorsi minimi

[gaten]

Salve ragazzi stò cercando di implementare l'algoritmo di Dijkstra per la ricerca del percorso minimo.
Praticamente l'algoritmo deve prende in input il grafo(sottoforma di matrice di interi), le coordinate della sorgente e le coordinate della destinazione e deve restituire le coordinate del prossimo nodo alla sorgente, che appartiene al percorso minimo.
Esempio se ho questa matrice 3x3:

121
310
001

Le coordinate della sorgente sono: (0,0) e la destinazione è (1,1) allora il percorso più breve sarà:

(0,0) -> (0,1) -> (1,1) ed il prossimo nodo alla sorgente è (0,1).

Io ho implementato così:

Dijkstra.h:

#include "heap.h"
#include "list.h"


#define INFINITO         999
#define NIL             -1
#define V_NON_RAGG      -1
#define ADJ_LEFT        0
#define ADJ_BOTT        1
#define ADJ_RIGHT        2
#define ADJ_UP            3




// prende in input una mappa un punto di partenza uno di arrivo e restituisce
// la destinazione
COORDINATE dijkstra(MAPPA *M, COORDINATE Sorg, COORDINATE Dest);

Dijkstra.c:

#include "dijkstra.h"
#include <assert.h>


#undef INFINITO
#define INFINITO 65535


int countVertices(MAPPA * M)
{
    int count = 0;
    int i,j;
    for (i=0; i<M->NRighe; i++)
    {
        for (j=0; j<M->NColonne; j++)
        {
            if(M->Mappa[i][j] != 0)
                count++;
        }
    }
    return count;
}


void fillVertices(MAPPA *M, VERTICE **Vertici, int count ){
    int index = 0; //scorre tutti i vertici NON MURI
    int counter = 0; //scorre tutti i vertici
    int i,j;
    for (i=0; i<M->NRighe; i++)
    {
        for (j=0; j<M->NColonne; j++)
        {
            M->VerticiNuovi[counter].predecessore = NULL;
            M->VerticiNuovi[counter].daicstraFlag = -1;
            M->VerticiNuovi[counter].me = NULL;
            if(M->Mappa[i][j] != 0)
            {
                Vertici[index] = &M->VerticiNuovi[counter];
                index++;
            }
            counter++;
        }
    }
    assert(index==count);
}


void initPesi(HEAP_NODE* heap,VERTICE **Vertici, int count, COORDINATE Sorg){
    int i;
    for(i=0; i<count; i++)
    {
        heap[i].Key = INFINITO;
        heap[i].Info = Vertici[i];


        //inizializza la struttura dati
        Vertici[i]->me = &heap[i]; //devo poter accedere a "key"


        //controlla se è il punto di partenza
        if(Sorg.X == Vertici[i]->Pos.X)
        if(Sorg.Y == Vertici[i]->Pos.Y)
        {
            heap[i].Key = 0; // this is not 0!
        }
    }
}


void doHeapRef(HEAP* q,int count){
    int i=0;
    for(;i<count;i++){
        HEAP_NODE * u = &q->A[i];
        VERTICE * v = (VERTICE*)u->Info;
        v->me = u;
    }
}




COORDINATE dijkstra(MAPPA *M, COORDINATE Sorg, COORDINATE Dest){


    int count = countVertices(M); //conta  i vertici NON MURO


    // Prende in un array un puntatore ad ogni vertice NON MURO
    VERTICE   **Vertici = (VERTICE**)malloc(sizeof(VERTICE*)*count);
    fillVertices(M,Vertici,count);


    // Inizializza tutti i pesi a INFINITO
    HEAP_NODE *A = (HEAP_NODE*)malloc(sizeof(HEAP_NODE)*count);
    initPesi(A,Vertici,count,Sorg);


    // Creo l'heap
    HEAP *Q;
    Q=Build_Min_Heap(A, count);


    VERTICE * destinazione = NULL; // destinazione finale


    // DAICSTRA ALGORITMO!! Come nel PDF e con il supporto di WIKIPEDIA
    while(HeapIsEmpty(Q)!=1){


        //StampHeap(Q);
        HEAP_NODE * u = ExtractMin(Q);
        //printf("\n Il minimo è : %d\n",u->Key);


        VERTICE * v = (VERTICE*)u->Info;
        if(destinazione == NULL)
            destinazione = v;


        if(u->Key == INFINITO){  // non ha trovato una strada.. il nemico è intrappolato in un muro circolare?
            printf("nemico intrappolato");
            destinazione = NULL;
            goto finalize;
        }


        int i=0;
        for(; i<4; i++){
            if(v->Adiacenti[i]!= NULL && v->Adiacenti[i]->daicstraFlag==-1){
                VERTICE * vicino = v->Adiacenti[i];
                vicino->daicstraFlag = 0;
                        // d[u]  +     w(u,v)
                int alt = u->Key + (v->Costo + vicino->Costo);
                //printf("alt: %d",alt);
                if( alt < vicino->me->Key ){
                    vicino->me->Key = alt;
                    vicino->predecessore = v;


                    if((destinazione->Pos.X == Dest.X)&&
                       (destinazione->Pos.Y == Dest.Y))
                    {
                        // ho finalmente trovato il nodo che cercavo, posso uscire!!!
                        //return destinazione;
                        //ora devo solo seguire a ritroso i predecessori.
                        //probabilmente è meglio crearsi una lista.
                        goto finalize;
                    }
                    destinazione = vicino;
                }
            }
        }


        //riordino lo heap..
        //TODO: servirebbe la funzione SetHeapKey(HEAP_NODE *, int newvalue);
        //ma non l'ho creata.. piccolo Hack
        HEAP *Q2 = Build_Min_Heap(Q->A, --count);
        doHeapRef(Q,count);
        free(Q);
        Q = Q2;
    }


finalize:


    free(Vertici);
    free(A);
    free(Q);
    if(destinazione!=NULL)
        return destinazione->predecessore->Pos;


    return Sorg;
}

heap.h:

#include <stdio.h>
#include <stdlib.h>


#define MAXDIM 100
#define N 6


typedef struct heap_node
{
    int Key;            // Chiave
    void *Info;         // Valore
} HEAP_NODE;




// tipo di dato HEAP
typedef struct heap
{
    int Dim;            // Dimensione array
    int MaxDim;         // Massima dimensione
    HEAP_NODE *A;       // Vettore
} HEAP;


typedef struct {
    int X;
    int Y;
} COORDINATE;


typedef struct _vertice
{
    COORDINATE Pos;
    int Costo;
    struct _vertice *Adiacenti[4]; // ho al massimo quattro adiacenti per ogni vertice
    struct _vertice *predecessore; // predecessore in daicstra
    int daicstraFlag;              // può essere -1,0 o 1
    HEAP_NODE * me;                // piccolo hack
} VERTICE;


typedef struct {
    int NRighe;
    int NColonne;
    int **Mappa;
    HEAP *Vertici;
    VERTICE * VerticiNuovi; //QUI
} MAPPA;

// testate e funzionanti.
HEAP           *InitHeap(HEAP_NODE *A, int Dim);            // Inizializzazione heap
void            MinHeapfy(HEAP *Heap, int i);
HEAP           *Build_Min_Heap(HEAP_NODE *A, int Dim);
HEAP_NODE   *ExtractMin(HEAP *Heap);                    // Estrazione del minimo
int          Left(int i);                                // Figlio Sinistro
int         Right(int i);                                // Figlio Destro
int          Parent(int i);                                // Padre
int             HeapIsEmpty(HEAP*);                         // VUOTO
void         Swap(HEAP_NODE *x, HEAP_NODE *y);            // Scambio di due elementi

main:

int main()
{
    int Map[N][N]={   {1,1,3,1,0,0},
                      {2,1,3,1,4,0},
                      {1,4,1,1,3,0},
                      {3,1,1,0,2,2},
                      {1,0,0,4,2,2},
                      {0,1,0,1,0,1},
                };
    int i,j;


    int **Lab=(int**)malloc(sizeof(int*)*N);
    for (i=0; i<N; i++)
        Lab[i]=(int*)malloc(sizeof(int)*N);




    for (i=0; i<N; i++){
        for (j=0; j<N; j++){
            Lab[i][j]=Map[i][j];
            printf("%d ",Map[i][j]);
        }
        printf("\n");
    }


    MAPPA *M=(MAPPA*)malloc(sizeof(MAPPA));
    M->Mappa = Lab;
    M->NRighe = N;
    M->NColonne = N;


    SalvaVertici(M);


    COORDINATE Sorg, Dest;
    Sorg.X=1;
    Sorg.Y=1;
    Dest.X=0;
    Dest.Y=0;


    COORDINATE attuali = Dest;
    StampaCoord(attuali);
    while(attuali.X!=Sorg.X || attuali.Y!=Sorg.Y){
        attuali = dijkstra(M,Sorg,attuali);
        StampaCoord(attuali);
    }


    system("pause");


    return 0;
}

Con questo main mi stampa:

(coordx 1, coordy 1)
(coordx 1, coordy 0)
(coordx 0, coordy 0)

invece dovrebbe essere (0,0), (0,1), (1,1)...