凸多边形碰撞的NFP算法实现

admin1

凸多边形碰撞的NFP算法实现

#include <windows.h>
#include <stdlib.h>
#include <stdio.h>
#include <conio.h>
#include <dos.h>
#define H 600
#define W 800


typedef struct node {   /*  定义的顶点结构　*/
   int x;
   int y;
   double value;
}NODE;

typedef struct edage{   /*  定义的边结构　*/
   int headX;
   int headY;
   int tailX;
   int tailY;
}EDGE;   

typedef struct polygon{  /*　定义的多边形结构 */
   NODE *Node;
   EDGE *Edge;
   int cx;
   int cy;
   int nNode;
}PolyGon;


PolyGon PolyGonA;       /*  声明多边形　A　变量　*/
PolyGon PolyGonB;       /*  声明多边形　B　变量　*/

NODE * trace;

int PolyALowX=0,PolyALowY=0;
int PolyBHighX=0,PolyBHighY=0;
int mycolor=1;

void init()
{
FILE *fp;
int i;

PolyGonA.nNode=0;
PolyGonB.nNode=0;

if( ( fp=fopen("d:/polygon.txt","r") ) ==NULL )
  {
    printf("Open file failed  ");
    exit(0);
  }

  fscanf( fp , "%d" , &PolyGonA.nNode);
  PolyGonA.Node = ( NODE *) malloc( sizeof(NODE)*PolyGonA.nNode );
  PolyGonA.Edge = ( EDGE *) malloc( sizeof(EDGE)*PolyGonA.nNode );

  for(i=0;i<PolyGonA.nNode;i++)
   fscanf( fp , "%d%d", &PolyGonA.Node[i].x , &PolyGonA.Node[i].y );  /*   读数据文件内容到多边形变量中 */



  fscanf( fp , "%d" , &PolyGonB.nNode);
  PolyGonB.Node = ( NODE *) malloc( sizeof(NODE)*PolyGonB.nNode );
  PolyGonB.Edge = ( EDGE *) malloc( sizeof(EDGE)*PolyGonB.nNode );

  for(i=0;i<PolyGonB.nNode;i++)
   fscanf( fp , "%d%d", &PolyGonB.Node[i].x , &PolyGonB.Node[i].y );  /*   读数据文件内容到多边形变量中 */

  fclose(fp);

  trace=(NODE *)malloc(sizeof(NODE)*(PolyGonA.nNode+PolyGonB.nNode+1)); /* 创建一个轨迹顶点列表 */

}

void Sort(NODE *Node , int n)                      /* 使用简单选择排序进行排序 */
{
  NODE tNode;
  int i,j,Min=0;

  for(i=0;i<n;i++)
  {
    Min=i;
    for(j=i;j<n;j++)
      if( Node[j].value<Node[Min].value )
          Min=j;
    if(Min!=i)
     {
       tNode=Node[i];
       Node[i]=Node[Min];
       Node[Min]=tNode;
     }
   }  
}


int ConnectNodeList(NODE * Node1, int nNode1, NODE *Node2 , int nNode2 , NODE * Node , int CONNECTSTYLE)
{
  int i,j;

  if(CONNECTSTYLE==0)
   {
    for(i=0 ; i<nNode1 ; i++)
      Node[i]=Node1[i];
    for(j=0;j<nNode2;j++)
      Node[i++]=Node2[j];
   }
  else if(CONNECTSTYLE==1)
   {
    for(i=0;i<nNode2;i++)
      Node[i]=Node2[i];

    for(j=0;j<nNode1;j++)
      Node[i++]=Node1[j];
   }
  return (nNode1+nNode2);
}

void CreatEdgeList()
{
  int i,j,k,npNode1=0,npNode2=0;
  int count=0;

  int CenterOfPolygonAX=0,CenterOfPolygonAY=0;
  int CenterOfPolygonBX=0,CenterOfPolygonBY=0;
  NODE *pNode1,*pNode2;


  for (i=0;i<PolyGonA.nNode;i++)
  {
    CenterOfPolygonAX+=PolyGonA.Node[i].x;
    CenterOfPolygonAY+=PolyGonA.Node[i].y;
  }
   CenterOfPolygonAX=CenterOfPolygonAX/PolyGonA.nNode;
   CenterOfPolygonAY=CenterOfPolygonAY/PolyGonA.nNode;        /* 得到多边形　A　的形心  */
   
  PolyGonA.cx=CenterOfPolygonAX;
  PolyGonA.cy=CenterOfPolygonAY;

  for (i=0;i<PolyGonB.nNode;i++)
  {
    CenterOfPolygonBX+=PolyGonB.Node[i].x;
    CenterOfPolygonBY+=PolyGonB.Node[i].y;
  }
   CenterOfPolygonBX=CenterOfPolygonBX/PolyGonB.nNode;
   CenterOfPolygonBY=CenterOfPolygonBY/PolyGonB.nNode;        /* 得到多边形　B　的形心  */

  PolyGonB.cx=CenterOfPolygonBX;
  PolyGonB.cy=CenterOfPolygonBY;

/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/

  i=0;
  j=0;

  pNode1=(NODE *)malloc(sizeof(NODE)*PolyGonA.nNode);
  pNode2=(NODE *)malloc(sizeof(NODE)*PolyGonA.nNode);  /*  临时顶点数组初始化 */

  for(k=0;k<PolyGonA.nNode;k++)