Робот должен добраться от А в пункт Б самым коротким путём

#include "stdafx.h"
class Graph
{int a[5][5];
int c[5][5];
int p[5][5];
 
public:
Graph()
{
int i,j,k;
for(i=0;i<=4;i++)
for(j=0;j<=4;j++) c[i][j]=999;
 
for(k=0;k<=6;k++)
{
scanf("%i%i",&i,&j);
scanf("%i",&c[i][j]);
}
}
void Floyd()
{
      int i,j,k;
for(i=0;i<=4;i++)
for(j=0;j<=4;j++) {a[i][j]=c[i][j];p[i][j]=-1;}
 
for(i=0;i<=4;i++) a[i][i]=0;
 
for(k=0;k<=4;k++)
for(i=0;i<=4;i++)
for(j=0;j<=4;j++)
if (a[i][k]+a[k][j]<a[i][j])
{
a[i][j]=a[i][k]+a[k][j];p[i][j]=k;
}
}
 
void print_a()
{
int i,j;
for(i=0;i<=4;i++)
{
for(j=0;j<=4;j++)    printf("%8i",a[i][j]);
printf("\n");
}
printf("\n");
}
void print_c()
{
int i,j;
for(i=0;i<=4;i++)
{
for(j=0;j<=4;j++)    printf("%8i",c[i][j]);
printf("\n");
}
printf("\n");
}
void print_p()
{
int i,j;
for(i=0;i<=4;i++)
{
for(j=0;j<=4;j++)    printf("%8i",p[i][j]);
printf("\n");
}
}
 
void path(int i,int j)
{
int k;
k=p[i][j];
if(k==-1) return;
path(i,k);
printf("%3i\n",k);
path(k,j);
}
};
 
int _tmain(int argc, _TCHAR* argv[])
{
Graph g;
g.Floyd();
g.print_c();
g.print_a();
g.print_p();
g.path(0,4);
return 0;
}