算法步骤简述：

1.计算图G加入新结点后的图G'，加入的新结点0到所有原结点之间距离为0，同时形成新的边集E'；

2.使用Bellman-Ford算法处理G'，并形成0结点到各结点的最小距离d。

3.如果Bellman-Ford算法检测出有负权回路则提示FALSE并退出，否则继续。

4.对所有G'中的顶点v,根据0结点到v的最小距离，将h(v)设置为这个值。

5.对所有的边w(u,v)，权值更新为w(u,v)+h(u)-h(v)

6.对图G中所有结点运行Dijkstra算法计算与其他顶点最短距离d'[u][v]

（此处假定G和w集合是分开存储的。直接使用G'也可以，因为0结点对其他结点是不可达的，但这显然浪费了计算时间。如果权值信息存在G'中，可以对G'进行操作，只不过跳过了0结点的处理）

7.原图G中最短距离d[u][v] = d'[u][v] +h(v)-h(u)

　　代码中有的地方没有优化，比如辅助结构vassist其实在Bellman-Ford算法和Dijkstra算法两个函数中用法稍微有所不同，而且成员变量在前者中只用了2个；同时松弛算法relax也有类似的情况。前者是简单的复用，后者直接用名字区分。

　　代码包含三部分：Bellman-Ford算法、Dijkstra算法、用二项堆实现的优先级数组（Dijkstra算法要用到）。以下是算法的C语言版本，测试实例同《算法导论》图25-1
复制代码 代码如下:
#include <stdio.h>
#include <stdlib.h>

#define U 65535
#define PARENT(i) ((i-1)/2)
#define LEFT(i) (2*(i)+1)
#define RIGHT(i) (2*(i)+2)
#define N 5

struct vertex {
int key;
struct vtable *adj;
};

struct vtable {
int key;//这个key是在vertex数组的序号
//struct vertext *v;
int w;
struct vtable *next;
};

struct vassist {
int d;
int p;
int key;
};

int insert(struct vertex *,int,int,int,int);
int walk(struct vertex *,int,int);
struct vassist *initialize_ss(int,int);
int relaxd(int *,int ,int ,int);
int relaxb(struct vassist *,int ,int ,int);
int build_min_heap(struct vassist *,int);
int min_heapify(struct vassist *, int ,int);
int heap_extract_min(struct vassist *,int);
int inheap(struct vassist *,int ,int );
int heap_decrease(struct vassist *,int ,int);
int dijkstra(struct vertex *,int,int,int **);
int bellman_ford(struct vertex *,int*, int,int);

int insert(struct vertex *p,int len,int i,int j,int w) {
struct vtable *q,*prev;
q = p[i].adj;
printf("key:%d\n",p[i].key);
prev = NULL;
while(q!=NULL) {
if (q->key == j) {
printf("error: v %d to %d already exist.\n",i,j);
return 0;
}
else {
prev = q;
q=q->next;
}
}
q = (struct vtable*)malloc(sizeof(struct vtable));
q->key = j;
q->w = w;
q->next = NULL;
if(prev!=NULL)
prev->next = q;
else
p[i].adj = q;
return 1;
}

int walk(struct vertex *p,int len,int i) {
struct vtable *q = p[i].adj;
while(q!=NULL) {
printf(" %d,w is %d\n",q->key,q->w);
q=q->next;
}
printf("\n");
}

struct vassist *initialize_ss(int size,int s) {
int i;
struct vassist *va;
va = (struct vassist *)malloc(size*sizeof(struct vassist));
for(i=0;i<size;i++) {
va[i].key = i;//建堆后i!=key
va[i].d = U;
va[i].p = -1;
}
va[s].d = 0;
return va;
}

//relax for dijkstra
int relaxd(int *p,int u,int v,int w) {//w=w(u,v)
if(p[v]>p[u]+w) {
p[v] = p[u]+w;
//为了简单处理,p使用的是数组
//没有父母标记
//如果想用父母标记,请将p改为一个自定义的结构体
}
return 1;
}

//relax for beltman_ford
int relaxb(struct vassist *va,int u,int v,int w) {//w=w(u,v)
if(va[v].d>va[u].d+w) {
va[v].d = va[u].d+w;
va[v].p = u;
}
return 1;
}

int bellman_ford(struct vertex *graph,int *h,int size,int s) {//算法要求不含源点可达的负权回路
int i,j;
struct vtable *p;
struct vassist *va;
va = initialize_ss(size,s);
for(i=1;i<size;i++)
for(j=0;j<size-1;j++) {
p = graph[j].adj;
while(p!=NULL) {
relaxb(va,j,p->key,p->w);
p=p->next;
}
}

printf("from %d,\n",s);
for(j=0;j<size;j++)
printf("to %d: %d\n",j,va[j].d);

for(j=0;j<size;j++) {//对0结点不必要
p = graph[j].adj;
while(p!=NULL) {
if(va[p->key].d>va[j].d+p->w)
return 0;
p = p->next;
}
}
for(j=1;j<=size;j++)
h[j] = va[j].d;
free(va);
h[0] = 0;
return 1;
}

int build_min_heap(struct vassist *va,int size) {//建堆
int i;
for (i =size/2-1; i>=0; i--)
min_heapify(va,i,size);

return 1;
}

int min_heapify(struct vassist *va, int i,int heap_size) {
int l,r,min;
struct vassist temp;
int tmin = U;
l = LEFT(i);
r = RIGHT(i);
if ((l < heap_size) &&(va[l].d<va[i].d)) {
min = l;
tmin = va[l].d;
}
else {
min = i;
tmin = va[i].d;
}
if ((r < heap_size) &&(va[r].d<va[min].d)) {
min = r;
tmin = va[r].d;
}
if (!(min == i)) {
temp.d = va[min].d;
temp.p = va[min].p;
temp.key = va[min].key;

va[min].d = va[i].d;
va[min].p = va[i].p;
va[min].key = va[i].key;

va[i].d = temp.d;
va[i].p = temp.p;
va[i].key = temp.key;

min_heapify(va,min,heap_size);
}
return 1;
}

int heap_extract_min(struct vassist *va,int heap_size) {
int min;
if ( heap_size<1 )
return -1;
min = va[0].key;
va[0].p = va[heap_size -1].p;
va[0].d = va[heap_size -1].d;
va[0].key = va[heap_size -1].key;
heap_size = heap_size -1;
min_heapify(va,0,heap_size);
return min;
}

int inheap(struct vassist *va,int heap_size,int j) {
int i;
for(i=0;i<heap_size;i++)
if(va[i].key == j)
return i;
return -1;
}

int heap_decrease(struct vassist *va,int i,int key_new) {
struct vassist temp;
if(key_new>va[i].d)
return 0;
va[i].d = key_new;
while((i>0)&&(va[PARENT(i)].d > va[i].d)) {
temp.d = va[i].d;
temp.p = va[i].p;
temp.key = va[i].key;
va[i].d = va[PARENT(i)].d;
va[i].p = va[PARENT(i)].p;
va[i].key = va[PARENT(i)].key;
va[PARENT(i)].d = temp.d;
va[PARENT(i)].p = temp.p;
va[PARENT(i)].key = temp.key;
i = PARENT(i);
}
return 1;
}

int dijkstra(struct vertex *graph,int len,int s,int **delta) {
int i,j,heap_size;
struct vtable *q;
struct vassist *va;
int *p;
p = (int *)malloc(len * sizeof(int));
for(i=0;i<len;i++)
p[i] = U;
p[s] = 0;
heap_size = len;

va = initialize_ss(len,s);
build_min_heap(va,heap_size);//va被拿去建堆,后续输出距离时不能再用了

while(heap_size>0) {
i = heap_extract_min(va,heap_size);
printf("node:%d\n",i);
heap_size--;
for(j=0;j<heap_size;j++)
printf("key:%d,d:%d, in array:%d\n",va[j].key,va[j].d,p[va[j].key]);
q = graph[i].adj;
while(q!=NULL) {
j=inheap(va,heap_size,q->key);
if(j>=0)
if(va[j].d>p[i]+q->w)
heap_decrease(va,j,p[i]+q->w);
relaxd(p,i,q->key,q->w);//其实可以合并heap_decreas和relax,不过为了接口简单没有这样做
printf("relax %d to %d ,w is %d\n",i,q->key,q->w);
q = q->next;
}
for(j=0;j<heap_size;j++)
printf("key:%d,d:%d, in array:%d\n",va[j].key,va[j].d,p[va[j].key]);
}
for(i=0;i<len;i++)
printf("from %d to %d, distance is %d\n",s,i,p[i]);

free(va);

for(i=0;i<len;i++) {
delta[s][i] = p[i];
}
free(p);

}

int **johnson(struct vertex *g, int n) {
int i,j;
int *h,**delta,**d;
struct vertex *gn;
struct vtable *p;
gn = (struct vertex *)malloc(n*sizeof(struct vertex));
h = (int *)malloc(n*sizeof(int));
delta = (int**)malloc(n*sizeof(int *));
d = (int**)malloc(n*sizeof(int *));
for(i=0;i<n;i++) {
delta[i]=(int*)malloc(n*sizeof(int));
d[i]=(int*)malloc(n*sizeof(int));
}
for(i=0;i<n;i++)
gn[i] = g[i];

for(i=1;i<n;i++)
insert(gn,n,0,i,0);
if(!bellman_ford(gn,h,n,0)) {
printf("the input graph contains a negative-weight cycle.\n");
return NULL;
}

for(i=0;i<n;i++) {
p = gn[i].adj;
while(p!=NULL) {
p->w = p->w+h[i]-h[p->key];
p=p->next;
}
}
for(i=0;i<n;i++)
walk(gn,n,i);

printf("before dijkstra\n");
for(i=1;i<n;i++) {
dijkstra(gn,n,i,delta);
for(j=1;j<n;j++)
d[i][j] = delta[i][j] + h[j] - h[i];

}
for(i=1;i<n;i++) {
for(j=1;j<n;j++)
printf("%d\t",d[i][j]);
printf("\n");
}
return d;
}

int main(){
int i,j;
int **d;
struct vertex vt[N+1];//为0结点的加入预留位置
for(i=0;i<N+1;i++) {
vt[i].adj = NULL;
vt[i].key = i;
}

insert(vt,N+1,1,2,3);
insert(vt,N+1,1,3,8);
insert(vt,N+1,1,5,-4);
insert(vt,N+1,2,4,1);
insert(vt,N+1,2,5,7);
insert(vt,N+1,3,2,4);
insert(vt,N+1,4,3,-5);
insert(vt,N+1,4,1,2);
insert(vt,N+1,5,4,6);
d = johnson(vt,N+1);

return 1;
}