Auxilio con ALGORITMO DE DIJKSTRA!!!!! :'c

DIJKSTRA!!!!
Estoy trabajando en un proyecto el entrega la ruta más corta de un nodo a otro dentro de un grafo
Este grafo lo tengo representado como una matriz de costos

He trabajado en ello y lo tengo "RELATIVAMENTE RESUELTO"

Lo que sucede es que me genera una ruta corta aunado a las aristas consiguientes

Por ejemplo si to quiero ir del nodo A al B...

A-------3-------B
| |
2 5
| |
C-------9-------D

La ruta sería A->B pero el programa toma A->C->D->B, esto porque la arista de A->C es menor que la de A->B

Como puedo resolver este problema????

Y si alguien lo tiene, me podría proporcionar un codigo usando el algoritmo de Dijkstra???

Añado mi codigo a mi publicación para que lo puedan observar y me puedan auxiliar en esto

En verdad me urge tener este codigo, de antemano muchas gracias y excelente dia c:

PD: SUELO TENER ALGO DE CODIGO BASURA EN MIS PROYECTOS POR SI ENCUENTRAN ALGO QUE NO SE OCUPE PARA NADA, ES UN MAL HABITO QUE TENGO Y SIGO TRABAJANDO EN ELLO :'c

Código

#include<stdio.h>
#include<conio.h>
#define max 6
#define INFINITO 999999
 
/*
---------------		SABADO 2 DE JUNIO 2018
***************			PROGRAMA: 0206
***************     	AXEL RODRIGUEZ
***************      METRO1 \ ESTRUCTURA DE DATOS
*/
int Menor(int distancia[max],int n, int visita_bandera[max])
{	int menor = INFINITO;
	int i;
	for (i=0;i<max;i++)
{
 
		if ( (distancia[i] ) < menor )
		{ 
			menor = distancia[i];
		}
	}
	return menor;                
}
 
void Dijkstra (int matriz[max][max], int nodo_inicial, int nodo_final)
{
	int i,j;
	int auxiliar[max][max];					   
	int distancia[max]={'\0'};		//VECTOR DONDE ESTARAN LAS DISTANCIAS GUARDADAS
	int analisis[max]={'\0'};			//VECTOR DE FILA QUE SE VA A ANALIZAR
	int visita[max]={'\0'};			//VECTOR EN EL QUE SE GUARDARAN LOS NODOS YA VISITADOS
	int visita_bandera[max]={'\0'};
	int z=1;
 
	for (i=0 ; i<max ; i++)			//DONDE NO EXISTA ADYACENCIA, LO MANDAMOS A UN VALOR MUUUUY ALTO PARA QUE NO SEA UTILIZADA
	{							//UTILIZAMOS UNA		//VECTOR EN EL QUE SE GUARDARAN LOS NODOS YA VISITADOS
		for (j=0 ; j<max ; j++)   
		{
			if (matriz[i][j] == 0)
			{
				auxiliar[i][j] = 999;
			}
			else{
				auxiliar[i][j] = matriz[i][j];
			}
		}
	}
 
 
distancia[0]=0;
visita[0]=nodo_inicial;
visita_bandera[nodo_inicial]=1;
 
do{		
rutina:
 
	for(i=0 ; i<max ; i++)	                      
	{
		analisis[i] = auxiliar [nodo_inicial][i];		//GUARDAMOS EN ANALISIS[] LA FILA COMPLETA DONDE ESTE EL NODO INICIAL
	}
 
	int menor_analisis;	
 
	menor_analisis = Menor (analisis, 6, visita_bandera);		//SE HACE USO DE LA FUNCION MENOR, ENVIANDOLE DE PARAMETROS EL VECTOR A ANALIZAR Y EL NUMERO 
													//DE ELEMENTOS DEL MISMO
 
	int vist,reaccion;		// EN VIST SE GUARDARA EL NODO VISITADO Y REACCION ES UNA BANDERA LA CUAL SERVIRA PARA DECIRNOS SI EL NODO YA FUE VISITADO
	for(i=0 ; i<max ; i++)	// O NO Y DE ESTE MODO EVITAR LOS BUCLES INFINITOS
	{                                                    	
		if (menor_analisis == auxiliar[nodo_inicial][i])
		{							// ENCUENTRA EL ELEMENTO EN LA MATRIZ DONDE ESTE EL DATO MENOR Y LA GUARDA EN VIST
			vist = i;
			break; 
		}
	}
 
	for(i=0 ; i<max ; i++)	// O NO Y DE ESTE MODO EVITAR LOS BUCLES INFINITOS
	{                                                    	
		if (menor_analisis == auxiliar[nodo_inicial][i])
		{							// ENCUENTRA EL ELEMENTO EN LA MATRIZ DONDE ESTE EL DATO MENOR Y LA GUARDA EN VIST
			vist = i;
 
		}
	}
 
	for (i=0 ; i<max ; i++)
	{
		if (vist == visita[i])
		{				
			reaccion=19;
		}
		else{
			reaccion=0;				// SI EL NODO NO HA SIDO VISITADO, REACCION ES 0, Y ESTOS NODOS SE MANDAN A INFINITO PARA QUE NO SE PUEDAN UTILIZAR 
			auxiliar[nodo_inicial][vist]=INFINITO;
			auxiliar[vist][nodo_inicial]=INFINITO;
		}
	}
 
	if(reaccion ==19)
	{
		auxiliar[vist][nodo_inicial]=INFINITO;
		auxiliar[nodo_inicial][vist]=INFINITO;
		goto rutina;
	}
 
	distancia[z]=menor_analisis;			// SE GUARDA EN DISTANCIA LA ARISTA DE MENOR TAMAÑO
	visita[z]=vist;							// SE GUARDA EN VISITA EL NODO YA VISITADO
 
	nodo_inicial = visita[z];				// SE IGUALA NODO INICIAL CON EL NODO ULTIMO VISITADO PARA QUE, CUANDO SE HAYA VISITADO 
	z++;									// EL NODO_FINAL, SE DETENGAN LAS ITERACIONES
 
}	while (nodo_inicial!=nodo_final);
 
	printf(" RUTA A SEGUIR --->  ");
	for (j=0 ; j<6 ; j++)
		{
 
			printf ("%d ", visita[j]+1);
			if (visita[j] == nodo_final)
			{
				break;
			}
		}
		int sumatoria=0;
	for (j=0;j<6;j++)
	{
		sumatoria=sumatoria+distancia[j];
	}
		printf("DISTANCIA RECORRIDA -->  %d", sumatoria);
 
 
}
 
int main(){
	int i,j;		// j-> fila    i-> columna
	//int matriz[5][5];
	int matriz [6][6]= {    0,4,2,0,0,0,
							4,0,1,5,0,0,
							2,1,0,8,9,0,
							0,5,8,0,2,6,
							0,0,9,2,0,2,
							0,0,0,6,2,0};	//ESTA MATRIZ ES EL GRAFO, EN ESENCIA ESTARIA DE ESTE MODO
 
		/*								
									 NODO 2	--- 5 ----NODO 4
									/   |		 	/	|	 \
 							   	  4	    |		 /		|	   \
								/       |	   /		|        \
					    	  NODO 1    1	 8			2		  NODO 6				
								  \     |  /			|         /
								    2   | /				|       /
								     \  |/				|      /
								      NODO 3 ----9-----	NODO 5						*/
	int auxiliar[6][6];					   
	int distancia[6]={'\0'};		//VECTOR DONDE ESTARAN LAS DISTANCIAS GUARDADAS
	int analisis[6]={'\0'};			//VECTOR DE FILA QUE SE VA A ANALIZAR
	int visita[6]={'\0'};			//VECTOR EN EL QUE SE GUARDARAN LOS NODOS YA VISITADOS
 
 
	for (i=0; i<6 ; i++)
	{
		for (j=0; j<6; j++)
		{
			printf(" %d", matriz[i][j]);
		}
		printf("\n");
	}
	printf("\n\n\n");
int nodo_inicial;
int nodo_final;
int z=1;	
mm:
	printf("Nodo inicial--> ");
	scanf("%d",&nodo_inicial);
	printf("Nodo final--> ");
	scanf("%d",&nodo_final);
	printf("\n\n");
 
	nodo_inicial=nodo_inicial-1;
	nodo_final=nodo_final-1;
 
	if (nodo_inicial<nodo_final){
	Dijkstra(matriz,nodo_inicial,nodo_final);
	printf("\n\n");  }
 
	if (nodo_inicial>nodo_final){
	Dijkstra(matriz,nodo_final,nodo_inicial);
	printf("\n\n"); 
	}
	goto mm;
 
	return 0;
}