次优路径算法

SPF算法找的是最优路径，下面的代码是在SPF基础上修改，查找的是次优路径

 

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

struct listnode {
  struct listnode *next;
  struct listnode *prev;
	void *data;
};

struct list 
{
  struct listnode *head;
  struct listnode *tail;
	unsigned long count;
	int (*cmp) (void *val1, void *val2);
	void (*del) (void *val);
};

#define NEXTNODE(X) ((X) = (X)->next)
#define LISTHEAD(X) ((X)->head)
#define LISTCOUNT(X) (((X) != NULL) ? ((X)->count) : 0)
#define listcount(X) (((X) != NULL) ? ((X)->count) : 0)
#define LIST_ISEMPTY(X) ((X)->head == NULL && (X)->tail == NULL)
#define list_isempty(X) ((X)->head == NULL && (X)->tail == NULL)
#define GETDATA(X) ((X)->data)

/* Prototypes. */
struct list *list_new(void);
void list_free (struct list *);
void listnode_free(struct listnode *node);
struct listnode *listnode_add (struct list *, void *);
struct listnode *listnode_add_sort (struct list *, void *);
struct listnode *listnode_add_before (struct list *, struct listnode *,void *);
int listnode_add_sort_nodup (struct list *, void *);
struct listnode *listnode_add_after (struct list *, struct listnode *, void *);
void *listnode_delete (struct list *, void *);
struct listnode *listnode_lookup (struct list *, void *);
void *listnode_head (struct list *);

void list_delete (struct list *);
void list_delete_all_node (struct list *);

/* For ospfd and ospf6d. */
void list_delete_node (struct list *, struct listnode *);

/* For ospf_spf.c */
void list_add_node_prev (struct list *, struct listnode *, void *);
void list_add_node_next (struct list *, struct listnode *, void *);
void list_add_list (struct list *, struct list *);

/* List iteration macro. */
#define LIST_LOOP(L,V,N) \
if (L)\
  for ((N) = (L)->head; (N); (N) = (N)->next) \
	if (((V) = (N)->data) != NULL)

/* List node add macro.  */
#define LISTNODE_ADD(L,N) \
  do { \
    (N)->next = NULL; \
    (N)->prev = (L)->tail; \
    if ((L)->head == NULL) \
      (L)->head = (N); \
    else \
      (L)->tail->next = (N); \
    (L)->tail = (N); \
  } while (0)

/* List node delete macro.  */
#define LISTNODE_DELETE(L,N) \
  do { \
    if ((N)->prev) \
      (N)->prev->next = (N)->next; \
    else \
      (L)->head = (N)->next; \
    if ((N)->next) \
      (N)->next->prev = (N)->prev; \
    else \
      (L)->tail = (N)->prev; \
  } while (0)





/********************************************************************************/
/* FUNCTION NAME: list_new                                                               */
/*                                                                              */
/* INPUTS:      1. None                                                      */
/*                                                                              */
/* RETURN:      linked list structure                                        */
/*                                                                           */
/*                                                                              */
/* NOTE:		new list structure                                              */
/*                                                                              */
/********************************************************************************/
struct list *list_new()
{
	struct list *list;
	
	list = (struct list *)malloc(sizeof(struct list));

	if (!list) 
	{
		printf("malloc list error\n");
		return NULL;
	}

	memset(list, 0, sizeof(struct list));
	return list;
}

/********************************************************************************/
/* FUNCTION NAME: list_free                                                               */
/*                                                                              */
/* INPUTS:      1. list  --  list structure                            */
/*                                                                              */
/* RETURN:                                                                   */
/*             None                                                          */
/*                                                                              */
/* NOTE:       free list structure                                        */
/*                                                                              */
/********************************************************************************/
void list_free(struct list *list)
{
	free(list);
}

/********************************************************************************/
/* FUNCTION NAME: newnode_new                                                               */
/*                                                                              */
/* INPUTS:      1. None                                                      */
/*                                                                              */
/* RETURN:      linked list node structure                                   */
/*                                                                           */
/*                                                                              */
/* NOTE:        new newnode                                                  */
/*                                                                              */
/********************************************************************************/
static struct listnode *listnode_new(void)
{
	struct listnode *node;
	
	node = (struct listnode *)malloc(sizeof(struct listnode));
	if (!node) 
	{
		printf("malloc node error\n");
		return NULL;
	}
		
	memset(node, 0, sizeof(struct listnode));
	return node;
}

/********************************************************************************/
/* FUNCTION NAME: newnode_free                                                               */
/*                                                                              */
/* INPUTS:      1. newnode  --  newnode structure                            */
/*                                                                              */
/* RETURN:                                                                   */
/*             None                                                          */
/*                                                                              */
/* NOTE:       free newnode structure                                        */
/*                                                                              */
/********************************************************************************/
void listnode_free(struct listnode *node)
{
	free(node);
}

/********************************************************************************/
/* FUNCTION NAME: newnode_add                                                               */
/*                                                                              */
/* INPUTS:      1. list  --  list                                         */
/*              2. val  --  data of newnode                                  */
/*                                                                              */
/* RETURN:      linked list node structure                                   */
/*                                                                           */
/*                                                                              */
/* NOTE:        add new node to list                                      */
/*                                                                              */
/********************************************************************************/
struct listnode * listnode_add(struct list *list,void *val)
{
  struct listnode *node;
  
  if ( (!list) || (!val) )
    return NULL;
  
  node = listnode_new ();
  if ( !node )
    return NULL;

  node->prev = list->tail;
  node->data = val;

  if (list->head == NULL)
    list->head = node;
  else
    list->tail->next = node;
  list->tail = node;

  list->count++;

  return node;
}

/* Add new node with sort function. */
struct listnode *
listnode_add_sort (struct list *list, void *val)
{
	struct listnode *n;
	struct listnode *_new;
	
	_new = (struct listnode *)malloc(sizeof(struct listnode));
	if (_new == NULL)
	{
		printf("malloc _new listnode error\n");
		return NULL;

	}
	_new->data = val;
	
	if (list->cmp) {
		for (n = list->head; n; n = n->next) {
			if ((*list->cmp) (val, n->data) < 0) {
				_new->next = n;
				_new->prev = n->prev;
				if (n->prev)
					n->prev->next = _new;
				else
					list->head = _new;
				n->prev = _new;
				list->count++;
				return _new;
			}
		}
	}
	_new->prev = list->tail;
	if (list->tail)
		list->tail->next = _new;
	else
		list->head = _new;
	list->tail = _new;
	list->count++;
	
	return _new;
}


/********************************************************************************/
/* FUNCTION NAME: newnode_delete                                                               */
/*                                                                              */
/* INPUTS:      1. list  --  list                                         */
/*              2. val  --  data of newnode                                  */
/*                                                                              */
/* RETURN:                                                                   */
/*              Pointer to void                                              */
/*                                                                              */
/* NOTE:        delete node from list                                     */
/*                                                                              */
/********************************************************************************/
void *listnode_delete(struct list *list,void *val)
{
	struct listnode *n;
	
	if ( (!list) || (!val) )
    	return NULL;
	
	for (n = list->head; n; n = n->next) {
		if (n->data == val) {
			if (n->prev)
				n->prev->next = n->next;
			else
				list->head = n->next;
			if (n->next)
				n->next->prev = n->prev;
			else
				list->tail = n->prev;
			list->count--;
			listnode_free(n);
			return val;
		}
	}
	return NULL;
}
/********************************************************************************/
/* FUNCTION NAME: list_first                                                               */
/*                                                                              */
/* INPUTS:      1. list  --  list                                         */
/*                                                                              */
/* RETURN:                                                                   */
/*              Pointer to void                                              */
/*                                                                              */
/* NOTE:        Return first node's data if it is there.                    */
/*                                                                              */
/********************************************************************************/
void *listnode_head (struct list *list)
{
	if (list->head)
		return list->head->data;
	return NULL;
}

void list_delete_all_node(struct list *list)
{
	struct listnode *n;
	struct listnode *next;
	
	for (n = list->head; n; n = next) {
		next = n->next;
		if (list->del)
			(list->del) (n->data);
		listnode_free(n);
	}
  list->head = list->tail = NULL;
  list->count = 0;
}
/********************************************************************************/
/* FUNCTION NAME: list_delete                                                               */
/*                                                                              */
/* INPUTS:      1. list  --  list                                         */
/*                                                                              */
/* RETURN:                                                                   */
/*             None                                                          */
/*                                                                              */
/* NOTE:        delete list                                               */
/*                                                                              */
/********************************************************************************/
void list_delete(struct list *list)
{
	struct listnode *n;
	struct listnode *next;
	
	for (n = list->head; n; n = next) {
		next = n->next;
		if (list->del)
			(list->del) (n->data);
		listnode_free(n);
	}
	list_free(list);
}
/*******************************************************************************
 *
 * FUNCTION	:	listnode_lookup
 *
 * PARAMS		:	list		- 
 *					data		- 
 *
 * RETURN		:	
 *
 * NOTE			:	Lookup the node which has given data.
 *
 * AUTHOR		:	SY.Liu
 *
 * DATE			:	2005.10.27 18:03:15
 *
*******************************************************************************/
struct listnode *listnode_lookup (struct list *list, void *data)
{
  struct listnode *node;

  for (node = list->head; node; node = node->next)
    if (data == node->data)
      return node;
  return NULL;
}

/* Delete the node from list.  For ospfd and ospf6d. */
void list_delete_node (struct list *list, struct listnode *node)
{
  if (node->prev)
    node->prev->next = node->next;
  else
    list->head = node->next;
  if (node->next)
    node->next->prev = node->prev;
  else
    list->tail = node->prev;
  list->count--;
  listnode_free (node);
}
/* ospf_spf.c */
void
list_add_node_prev (struct list *list, struct listnode *current, void *val)
{
  struct listnode *node;

  node = listnode_new ();
  node->next = current;
  node->data = val;

  if (current->prev == NULL)
    list->head = node;
  else
    current->prev->next = node;

  node->prev = current->prev;
  current->prev = node;

  list->count++;
}

/* ospf_spf.c */
void
list_add_node_next (struct list *list, struct listnode *current, void *val)
{
  struct listnode *node;

  node = listnode_new ();
  node->prev = current;
  node->data = val;

  if (current->next == NULL)
    list->tail = node;
  else
    current->next->prev = node;

  node->next = current->next;
  current->next = node;

  list->count++;
}


#define ARRAYLEN(x) sizeof(x)/(sizeof(*x))
#define NUM 8
//#define INT_MAX 20000000

#define show 1



typedef struct Node
{
	int Weight;
	char vertex;
	struct Node *next;
}*pNode;

typedef struct NodeLink 
{
	int parent;
	int parent_sec;
	int dist;
	int dist_sec;
	char nexthop;
	char vertex;
	struct Node *link;
	struct list *candidate;
}*pNodeLink;

struct candidate_node
{
	int parent;
	int dist;
	char nexthop;
};

typedef struct Heap
{
	int pos[NUM];
	int HeapSize;
}*pHeap;

int Key[NUM][2];

#define swap(a, b)\
do {\
	int t = (a);\
	a = b;\
	b = t;\
}while(0)


/*
A:5.5.5.5
B:192.85.1.1
C:1.1.1.1
D:2.2.2.1
E:192.0.0.1
F:8.8.8.8
G:193.85.1.1
H:192.0.0.2
*/

pNodeLink CreatDGraph()
{
	char ListTable[NUM][3]={
		{'B','C','D'},/* A */
		{'A','E'},/* B */
		{'A','F'},/* C */
		{'A','F'},/* D */
		{'B'},/* E */
		{'C','D','G'},/* F */
		{'F','H'},/* G */
		{'G'}/* H */
	};
	char Vertex[]={'A','B','C','D','E','F','G','H'};
	int Weight[]={
		1,1,10,
			0,0,
			0,0,
			0,0,
			1,
			1,1,1,
			0,0,
			1
	};
	int Size[]={3,2,2,2,1,3,2,1};
	int n = 0;


	pNodeLink pN = (pNodeLink)malloc(sizeof(struct NodeLink) * NUM);//new NodeLink[NUM];/* 一共有六个结点 */

	for (int i=0;i<NUM;i++)
	{
	   pN[i].link = NULL;
	   pN[i].vertex = Vertex[i];/* 当前结点的名字*/

	   for (int j=0;j<Size[i];j++)
	   {
			pNode NewNode = (pNode)malloc(sizeof(struct Node));//new Node[1];
			NewNode->Weight = Weight[n++];//权值 
			NewNode->vertex = ListTable[i][j];//与当前结点vertex连接的其他结点的名称 
			NewNode->next = pN[i].link;
			pN[i].link = NewNode;
	   }
	}

	return pN;

}


int FindIndex(char c)
{
	return (int)(c-'A');
}

char FindVertex(int index)
{
	return (char)('A' + index);
}

void KeySwap(pHeap p,int i,int j)
{
	swap(Key[p->pos[i]][1],Key[p->pos[j]][1]);
	swap(p->pos[i],p->pos[j]);
}


void MinHeap(pHeap p,int i)
{
	int L = i*2+1;
	int R = i*2+2;
	int smallest;

	if (L<p->HeapSize&&Key[p->pos[L]][0]<Key[p->pos[i]][0])
	   smallest = L;
	else
	   smallest = i;

	if (R<p->HeapSize&&Key[p->pos[R]][0]<Key[smallest][0])
	   smallest = R;

	if (smallest != i)
	{
	   KeySwap(p,smallest,i);
	   MinHeap(p,smallest);
	}
}

void HeapDecrease(pHeap p,int pos,int weight)
{
	Key[pos][0] = weight; //pos 为pNodeList的顺序
	int i = Key[pos][1]; //i 为 pos 在堆中的顺序

	/* p->pos[i] 堆中的位置为i的 元素在key中的索引 */
	while (i>=0&&Key[p->pos[i]][0]<Key[p->pos[i/2]][0])
	{
	   KeySwap(p,i,i/2);
	   i /=2;
	}
}

void MinHeapInsert(pHeap p,int pos,int weight)
{
	if (Key[pos][1]>=p->HeapSize) //不在堆中
	{
		printf("vertex %c add to MinHep\n",FindVertex(pos));
		p->pos[p->HeapSize] = pos;
		Key[pos][0] = INT_MAX;
		Key[pos][1] = p->HeapSize;
		p->HeapSize++;
		HeapDecrease(p, pos, weight);
	}
	else
	{
		//在堆中不进行任何处理
	}
}

int ExtractMin(pHeap p)
{
	int pos = p->pos[0];

	KeySwap(p,0,p->HeapSize-1);	/*  第0个为最小值，与最后一个交换 */
	p->HeapSize--;
	MinHeap(p,0);	/* 再执行最小堆 */
	return pos;
}

void BuildMinHeap(pHeap p)
{
	for (int i=p->HeapSize/2;i>=0;i--)
	   MinHeap(p,i);
}

void InitHeap(pHeap pH,pNodeLink pN)
{
	for (int i=0;i<NUM;i++)
	{
	   Key[i][0] = pN[i].dist;
	   Key[i][1] = i;
	   pH->pos[i] = i;
	}
	pH->HeapSize = NUM;

	BuildMinHeap(pH);
}

/*
int vertex_cmp(void *v1, void *v2)
{
	struct candidate_node * a = (struct candidate_node *)v1;
	struct candidate_node * b = (struct candidate_node *) v2;
	
	return b->dist - a->dist;
}
*/
//?start here 
void Initalize_Single_Source(pNodeLink p,int n)
{
	for (int i=0;i<NUM;i++)
	{
	   p[i].dist = INT_MAX;
	   p[i].dist_sec = INT_MAX;
	   p[i].parent = 0;
	   p[i].parent_sec = 0;
	   p[i].nexthop = 0;
	   p[i].candidate = list_new();
	   //p[i].candidate->cmp = vertex_cmp;
	}

	p[n].dist = 0; //从第n个点开始出发
}


int Add2Candidate(pNodeLink p,int v,int dist,int pnt,char nexthop)
{
	struct candidate_node  *can_node;
	struct listnode *node;

	if (nexthop == 0)
		nexthop = FindVertex(v);

	for (node = p[v].candidate->head;node;node = node->next)
	{
		if (can_node = (struct candidate_node*)node->data);
		if (can_node->parent == pnt && can_node->dist == dist && can_node->nexthop == nexthop)
			return 0;
	}

	can_node = (struct candidate_node *)malloc(sizeof(struct candidate_node));
	//can_node->vertex = FindVertex(v);
	can_node->dist = dist;
	can_node->parent = pnt;
	can_node->nexthop = nexthop;
	listnode_add(p[v].candidate, can_node);

	printf("vertex %c add candidate node ,parent = %c, dist = %d nexthop = %c\n",FindVertex(v),FindVertex(pnt),dist,nexthop);
	return 1;
}

char set_nexthop(int u,int v)
{
	if (u == 0)
		return (char)('A' + v);
	return 0;

}


/* 将结点u下的所有 Candidate结点 计算加入到结点v中 ,weight 为 u到v的距离 */
int Calc_NextNode(pNodeLink p, int u, int v,int weight)
{
	struct listnode *node;
	struct candidate_node *can_node;
	int mindist = p[v].dist_sec;
	int secmin = mindist;
	char sechop = 'X';
	char minhop = 'X';
	char nexthop = 'X';
	int rc = 0;
	int pnt = 0;
	
	/* 遍历父节点u的所有候选结点*/
	for (node = p[u].candidate->head;node;node = node->next)
	{
		can_node = (struct candidate_node *)node->data;
		if (can_node->parent == v)	/* 去掉环 */ {
			printf("skip loop\n");
			continue;
		}
		

		printf("LOOP u=%c, v= %c, weight %d, dist = %d, pnt = %c, nexthop = %c\n",FindVertex(u),FindVertex(v), weight, can_node->dist, FindVertex(can_node->parent), can_node->nexthop);
		
		/* 找出最优和次优的路由，如果存在次优的，则替换之 */
		if (mindist > can_node->dist + weight)
		{
			mindist = can_node->dist + weight;
			//pnt = FindIndex(can_node->vertex);
			minhop = can_node->nexthop;
		} else if (secmin > can_node->dist + weight){
			secmin = can_node->dist + weight;
			sechop = can_node->nexthop;
		}
	

		if (can_node->dist + weight > p[v].dist_sec)
			continue;
		
		nexthop = can_node->nexthop;
		Add2Candidate(p, v, can_node->dist + weight, u, nexthop);
	}
	
	printf("Calc_NextNode out mindist %d, dist_sec %d, p[v].nexthop: %c, minhop:%c, secdist %d, sechop %c\n", 
		mindist, p[v].dist_sec, (p[v].nexthop), (minhop), secmin, sechop);
	
	if (secmin != p[v].dist_sec)
//	if (mindist != p[v].dist_sec)
	{
		printf("set vertex %c secmin = %d\n",FindVertex(v),secmin);
		if (p[v].dist_sec > secmin)
			rc++;

		p[v].dist_sec = secmin;
		p[v].parent_sec = u;
	} else if (mindist != p[v].dist_sec && p[v].nexthop != minhop)
	{
		//存在路径路径dist相等，但是下一跳不相等的情况  
		printf("set vertex %c sec_dist = %d\n",FindVertex(v),mindist);
		if (p[v].dist_sec > mindist)
			rc++;

		p[v].dist_sec = mindist;
		p[v].parent_sec = u;
	}

	return rc;
	
}


int Relax(pHeap pH,pNodeLink p,int u,int v)
{
	int weight;
	int rc = 0;
	struct listnode *node;
	int set_second = 0;
	int mindist = INT_MAX;
	int pnt;
	int old_dist;

	char nexthop = set_nexthop(u,v);

	for (pNode ptr = p[u].link;ptr;ptr=ptr->next)
	{
	   if (ptr->vertex == p[v].vertex)
		weight = ptr->Weight;
	}

	if (v == 0)
		return 0;

	if (p[v].dist > (p[u].dist + weight))
	{
		printf("p[%c].dist > p[%c] + weight\n", FindVertex(v), FindVertex(u));
		old_dist = p[v].dist;
		p[v].dist = p[u].dist + weight;
		p[v].parent = u;
		p[v].nexthop = (p[u].nexthop == 0)?FindVertex(v):p[u].nexthop;
		Add2Candidate(p, v, p[v].dist, u, p[v].nexthop);

		rc = 1;

		if (p[v].dist_sec >= old_dist)
		{
			if (0 != Calc_NextNode(p,u,v,weight))
				MinHeapInsert(pH,v,p[v].dist);
		}
	}
	else
	{
		if (p[v].dist_sec >= p[u].dist + weight )
		{
			if (0 != Calc_NextNode(p,u,v,weight))
				MinHeapInsert(pH,v,p[v].dist);
		}
	}

	
	return rc;
}




void Dijkstra(pNodeLink p,char c)
{
	int i;
	Initalize_Single_Source(p,FindIndex(c));

	pHeap pH = (pHeap)malloc(sizeof(struct Heap));//new Heap[1];
	InitHeap(pH,p);

	while (pH->HeapSize!=0)
	{
	   int index = ExtractMin(pH);

	   printf("Extract %c\n",FindVertex(index));

	   for (pNode ptr = p[index].link;ptr;ptr = ptr->next)
	   {
			int pos = FindIndex(ptr->vertex);
			printf("now check (%c->%c) u.dist = %d, u.sec_dist = %d, v.dist = %d, v.sec_dist = %d\n",
				FindVertex(index),FindVertex(pos),p[index].dist,p[index].dist_sec,p[pos].dist,p[pos].dist_sec);
			if (Relax(pH,p,index,pos))
				HeapDecrease(pH,pos,p[pos].dist);
	   }
	}

	for (i=0;i<NUM;i++)
	{
		printf("dist(%c,%c)=%d\n", c, p[i].vertex, p[i].dist);
	   //cout<<"dist("<<c<<","<<p[i].vertex<<")"<<" = "<<p[i].dist<<endl;
	}

	for (i=0;i<NUM;i++)
	{
		printf("%c 'parent %c'\n",p[i].vertex, FindVertex(p[i].parent));
		//cout<<p[i].vertex<<"'parent  = "<<<<endl;
	}

	for (i=0;i<NUM;i++)
	{
		printf("dist_sec(%c,%c)=%d\n", c, p[i].vertex, p[i].dist_sec);
		//cout<<"dist_sec("<<c<<","<<p[i].vertex<<")"<<" = "<<p[i].dist_sec<<endl;
	}

	for (i=0;i<NUM;i++)
	{
		printf("%c 'parent = %c'\n", p[i].vertex, FindVertex(p[i].parent_sec));
		//cout<<<<"'parent = "<<<<endl;
	}
}




int main()
{
	int i = 0;
	int parent = -1;
	int child = -1;
	pNodeLink p = CreatDGraph();
	struct list *list;
	struct listnode* _node;
	candidate_node  *can_node,*dest_node;

	Dijkstra(p,'A');


	printf("shortest distance path------\n");
	for (i = 1;i<NUM;i++)
	{
		parent = -1;
		parent = p[i].parent;
		printf("%c<-%c",FindVertex(i),FindVertex(parent));
		if (parent != 0) 
		{
			do
			{
				parent = p[parent].parent;		
				printf("<-%c",FindVertex(parent));
			}while (parent != 0);
		}
		
		printf(" %d nexthop = %c\n",p[i].dist,p[i].nexthop);

	}

	printf("\n\n");

	for (i = 1;i<NUM;i++)
	{
		list = p[i].candidate;
		int sec_dist = INT_MAX;
		dest_node = NULL;
	
		if (list->count!=0)
			printf("vertex %c's \n",FindVertex(i));


		for (_node = list->head;_node;_node = _node->next)
		{
			can_node = (struct candidate_node *)_node->data;
			if (can_node!=NULL)
				printf("dist = %d, pnt = %c, nexthop = %c\n",can_node->dist, FindVertex(can_node->parent), can_node->nexthop);
		}
	}


	printf("\n\n");
	
	for (i = 1;i<NUM;i++)
	{
		list = p[i].candidate;
		int sec_dist = INT_MAX;
		dest_node = NULL;
	
		if (list->count!=0)
			printf("vertex %c's \n",FindVertex(i));


		for (_node = list->head;_node;_node = _node->next)
		{
			can_node = (struct candidate_node *)_node->data;
			if (can_node!=NULL)
			{
				if (can_node->parent == p[i].parent && can_node->dist == p[i].dist)
					continue;

				if (sec_dist > can_node->dist && \
					((can_node->dist > p[i].dist) || (can_node->dist == p[i].dist && can_node->nexthop != p[p[i].parent].nexthop)))
				{
					dest_node = can_node;
					sec_dist = dest_node->dist;
				}
			}
		}

		if (dest_node) {
			can_node = dest_node;
			printf(" dist = %d, next hop = %c\n",can_node->dist,can_node->nexthop);
		}

	}

	getchar();
	return 0;
}