|
最近我写了一个关于凸多边形碰撞的NFP算法的小程序,这里把代码传上来大家看一看。程序是用devc++实现的windowsAPI程序。
#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++)
{
if(PolyGonA.Node[k].x>=CenterOfPolygonAX)
{
pNode1[i]=PolyGonA.Node[k];
if(pNode1[i].x==CenterOfPolygonAX )
{
if(pNode1[i].y>CenterOfPolygonAY)
pNode1[i].value=1000;
else
pNode1[i].value=-1000;
}
else
{
pNode1[i].value= (double)( pNode1[i].y - CenterOfPolygonAY)/(pNode1[i].x - CenterOfPolygonAX);
}
i++;
}
else
{
pNode2[j]=PolyGonA.Node[k];
pNode2[j].value= (double)( pNode2[j].y - CenterOfPolygonAY)/(pNode2[j].x - CenterOfPolygonAX);
j++;
}
}
npNode1=i;
npNode2=j;
Sort(pNode1,npNode1);
Sort(pNode2,npNode2);
count=ConnectNodeList(pNode1, npNode1, pNode2 , npNode2 , PolyGonA.Node , 0);
free(pNode1);
free(pNode2);
for(i=0;i<count;i++) /* 对节点进行编号,初始化边列表 */
{
PolyGonA.Edge[i].headX=PolyGonA.Node[i].x;
PolyGonA.Edge[i].headY=PolyGonA.Node[i].y;
if(i<count-1) /* 当 i<=count-2 时 */
{
PolyGonA.Edge[i].tailX=PolyGonA.Node[i+1].x;
PolyGonA.Edge[i].tailY=PolyGonA.Node[i+1].y;
}
else /* 当 i=count-1 时 */
{
PolyGonA.Edge[i].tailX=PolyGonA.Node[(i+1)%count].x;
PolyGonA.Edge[i].tailY=PolyGonA.Node[(i+1)%count].y;
}
}
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
i=0,j=0;
pNode1=(NODE *)malloc(sizeof(NODE)*PolyGonB.nNode);
pNode2=(NODE *)malloc(sizeof(NODE)*PolyGonB.nNode);
for(k=0;k<PolyGonB.nNode;k++)
{
if(PolyGonB.Node[k].x>=CenterOfPolygonBX)
{
pNode1[i]=PolyGonB.Node[k];
if(pNode1[i].x==CenterOfPolygonBX )
{
if(pNode1[i].y>CenterOfPolygonBY)
pNode1[i].value=1000;
else
pNode1[i].value=-1000;
}
else
{
pNode1[i].value= (double)( pNode1[i].y - CenterOfPolygonBY)/(pNode1[i].x - CenterOfPolygonBX);
}
i++;
}
else
{
pNode2[j]=PolyGonB.Node[k];
pNode2[j].value= (double)( pNode2[j].y - CenterOfPolygonBY)/(pNode2[j].x - CenterOfPolygonBX);
j++;
}
}
npNode1=i;
npNode2=j;
Sort(pNode1,npNode1);
Sort(pNode2,npNode2);
count=ConnectNodeList(pNode1, npNode1, pNode2 , npNode2 , PolyGonB.Node , 1);
free(pNode1);
free(pNode2);
for(i=0;i<count;i++) /* 对节点进行编号,初始化边列表 */
{
PolyGonB.Edge[i].headX=PolyGonB.Node[i].x;
PolyGonB.Edge[i].headY=PolyGonB.Node[i].y;
if(i<count-1) /* 当 i<=count-2 时 */
{
PolyGonB.Edge[i].tailX=PolyGonB.Node[i+1].x;
PolyGonB.Edge[i].tailY=PolyGonB.Node[i+1].y;
}
else /* 当 i=count-1 时 */
{
PolyGonB.Edge[i].tailX=PolyGonB.Node[(i+1)%count].x;
PolyGonB.Edge[i].tailY=PolyGonB.Node[(i+1)%count].y;
}
}
/*@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@*/
}
void DrawPolyGon(HDC hDC,EDGE * Edge, int n)
{
int i,M,N,tmpX=0,tmpY=0;
N=H;
tmpX=Edge[0].headX;
tmpY=N-Edge[0].headY;
MoveToEx(hDC,tmpX,tmpY,NULL);
for(i=0;i<n;i++)
{
tmpX=Edge[i].tailX;
tmpY=N-Edge[i].tailY;
LineTo(hDC,tmpX,tmpY);
}
}
void Adjust()
{
int i,j,Min=0,Max=0;
int dx=0,dy=0;
for(i=1;i<PolyGonA.nNode;i++)
if(PolyGonA.Node[i].y<=PolyGonA.Node[Min].y)
Min=i;
PolyALowX=PolyGonA.Node[Min] .x;
PolyALowY=PolyGonA.Node[Min] .y; /* 得到A多边形的最底点坐标 */
for(i=1;i<PolyGonB.nNode;i++)
if(PolyGonB.Node[i].y>=PolyGonB.Node[Max].y)
Max=i;
PolyBHighX=PolyGonB.Node[Max] .x;
PolyBHighY=PolyGonB.Node[Max] .y; /* 得到B多边形的最高点坐标 */
dx=PolyBHighX-PolyALowX;
dy=PolyBHighY-PolyALowY;
for(i=0;i<PolyGonA.nNode;i++)
{
PolyGonA.Node[i].x=PolyGonA.Node[i].x+dx;
PolyGonA.Node[i].y=PolyGonA.Node[i].y+dy;
PolyGonA.Edge[i].headX=PolyGonA.Edge[i].headX+dx;
PolyGonA.Edge[i].headY=PolyGonA.Edge[i].headY+dy;
PolyGonA.Edge[i].tailX=PolyGonA.Edge[i].tailX+dx;
PolyGonA.Edge[i].tailY=PolyGonA.Edge[i].tailY+dy;
}
PolyGonA.cx=PolyGonA.cx+dx;
PolyGonA.cy=PolyGonA.cy+dy;
}
int JudgeAcross( int LP1X, int LP1Y, int LP2X, int LP2Y, int CPolyGonAX, int CPolyGonAY, int CPolyGonBX, int CPolyGonBY)
/* 判断两个点是否在 一条直线的两侧 */
{
double k=0;
double f1=0,f2=0;
if(LP1X==LP2X)
{
if( (CPolyGonAX>LP1X && CPolyGonBX<LP1X) || (CPolyGonAX<LP1X &&CPolyGonBX>LP1X) )
{
return 1;
}
else
return 0;
}
else
{
k=(double)(LP1Y-LP2Y)/(LP1X-LP2X);
f1=k*(CPolyGonAX-LP1X)-CPolyGonAY+LP1Y;
f2=k*(CPolyGonBX-LP1X)-CPolyGonBY+LP1Y;
if(f1*f2<0)
return 1;
else
return 0;
}
}
int ChooseEdge(EDGE Edge, int x, int y)
{
int i,j;
int LP1X,LP1Y,LP2X,LP2Y;
int NodeAX=0,NodeAY=0;
int result=-1;
LP1X=Edge.headX;
LP1Y=Edge.headY;
LP2X=Edge.tailX;
LP2Y=Edge.tailY;
for(j=0;j<PolyGonA.nNode;j++)
{
NodeAX=PolyGonA.Node[j].x;
NodeAY=PolyGonA.Node[j].y;
if(NodeAX==x&&NodeAY==y)
continue;
result=JudgeAcross(LP1X,LP1Y,LP2X,LP2Y,NodeAX,NodeAY,PolyGonB.cx,PolyGonB.cy);
if(result==0)
return 0;
}
return 1;
}
void DrawPolyGonATo(HDC hDC, int dx, int dy)
{
int i,N;
int tmpx=0,tmpy=0;
N=H;
tmpx=PolyGonA.cx;
tmpy=PolyGonA.cy;
for(i=0;i<PolyGonA.nNode;i++)
{
PolyGonA.Node[i].x=PolyGonA.Node[i].x+dx;
PolyGonA.Node[i].y=PolyGonA.Node[i].y+dy;
PolyGonA.Edge[i].headX=PolyGonA.Edge[i].headX+dx;
PolyGonA.Edge[i].headY=PolyGonA.Edge[i].headY+dy;
PolyGonA.Edge[i].tailX=PolyGonA.Edge[i].tailX+dx;
PolyGonA.Edge[i].tailY=PolyGonA.Edge[i].tailY+dy;
}
PolyGonA.cx=PolyGonA.cx+dx;
PolyGonA.cy=PolyGonA.cy+dy;
DrawPolyGon(hDC,PolyGonA.Edge,PolyGonA.nNode);
/* moveto(tmpx,N-tmpy);
setlinestyle(1,1,1);
setcolor(10);
lineto(PolyGonA.cx,N-PolyGonA.cy);
*/
MoveToEx(hDC,PolyGonA.cx,N-PolyGonA.cy,NULL);
LineTo(hDC,PolyGonA.cx,N-PolyGonA.cy);
/* putpixel(PolyGonA.cx,N-PolyGonA.cy,4); */
}
void TurnPolyGon(HDC hDC)
{
int i,result=-1;
int CountA=0,CountB=0;
int indexA=0,indexB=0;
int Min=0,Max=0;
int dx=0,dy=0;
int nTrace=0;
int tpx,tpy;
HPEN hPen;
for(i=1;i<PolyGonA.nNode;i++)
if(PolyGonA.Node[i].y<=PolyGonA.Node[Min].y)
Min=i;
for(i=0;i<PolyGonA.nNode;i++)
if(PolyGonA.Edge[i].headX==PolyGonA.Node[Min].x && PolyGonA.Edge[i].headY==PolyGonA.Node[Min].y )
indexA=i;
for(i=1;i<PolyGonB.nNode;i++)
if(PolyGonB.Node[i].y>=PolyGonB.Node[Max].y)
Max=i;
for(i=0;i<PolyGonB.nNode;i++)
if(PolyGonB.Edge[i].headX==PolyGonB.Node[Max].x && PolyGonB.Edge[i].headY==PolyGonB.Node[Max].y )
indexB=i;
trace[0].x=PolyGonA.cx;
trace[0].y=PolyGonA.cy;
nTrace++; /* 本段代码记录轨迹顶点的起点坐标 */
Sleep(2000);
while(1)
{
result=ChooseEdge( PolyGonB.Edge[indexB], PolyGonA.Edge[indexA].headX , PolyGonA.Edge[indexA].headY );
if(result)
{
dx=PolyGonB.Edge[indexB].tailX-PolyGonB.Edge[indexB].headX;
dy=PolyGonB.Edge[indexB].tailY-PolyGonB.Edge[indexB].headY;
if(nTrace<=PolyGonA.nNode+PolyGonB.nNode)
{
trace[nTrace].x=trace[nTrace-1].x+dx;
trace[nTrace].y=trace[nTrace-1].y+dy;
nTrace++;
}
hPen=CreatePen(PS_SOLID,1,RGB(192,192,192));
SelectObject(hDC,hPen);
DrawPolyGon(hDC,PolyGonA.Edge,PolyGonA.nNode);
tpx=PolyGonA.cx;
tpy=PolyGonA.cy;
hPen=CreatePen(PS_SOLID,1,RGB(255,255,0));
SelectObject(hDC,hPen);
DrawPolyGon(hDC,PolyGonB.Edge,PolyGonB.nNode);
hPen=CreatePen(PS_SOLID,1,RGB(255,0,0));
SelectObject(hDC,hPen);
DrawPolyGonATo(hDC,dx,dy);
MoveToEx(hDC,tpx,H-tpy,NULL);
hPen=CreatePen(PS_DASHDOT,1,RGB(0,255,0));
SelectObject(hDC,hPen);
LineTo(hDC,PolyGonA.cx,H-PolyGonA.cy);
indexB=(indexB+1)%PolyGonB.nNode;
CountB++;
Sleep(1000);
}
else
{
dx=PolyGonA.Edge[indexA].headX-PolyGonA.Edge[indexA].tailX;
dy=PolyGonA.Edge[indexA].headY-PolyGonA.Edge[indexA].tailY;
if(nTrace<=PolyGonA.nNode+PolyGonB.nNode)
{
trace[nTrace].x=trace[nTrace-1].x+dx;
trace[nTrace].y=trace[nTrace-1].y+dy;
nTrace++;
}
hPen=CreatePen(PS_SOLID,1,RGB(192,192,192));
SelectObject(hDC,hPen);
DrawPolyGon(hDC,PolyGonA.Edge,PolyGonA.nNode);
tpx=PolyGonA.cx;
tpy=PolyGonA.cy;
hPen=CreatePen(PS_SOLID,1,RGB(255,255,0));
SelectObject(hDC,hPen);
DrawPolyGon(hDC,PolyGonB.Edge,PolyGonB.nNode);
hPen=CreatePen(PS_SOLID,1,RGB(255,0,0));
SelectObject(hDC,hPen);
DrawPolyGonATo(hDC,dx,dy);
MoveToEx(hDC,tpx,H-tpy,NULL);
hPen=CreatePen(PS_DASHDOT,1,RGB(0,255,0));
SelectObject(hDC,hPen);
LineTo(hDC,PolyGonA.cx,H-PolyGonA.cy);
indexA=(indexA+1)%PolyGonA.nNode;
CountA++;
Sleep(1000);
}
if(CountA>=PolyGonA.nNode && CountB>=PolyGonB.nNode)
break;
}
}
void DrawTrace(HDC hDC)
{
int i;
int tmpx,tmpy;
int N;
N=H;
tmpx=trace[0].x;
tmpy=N-trace[0].y;
MoveToEx(hDC,tmpx,tmpy,NULL);
LineTo(hDC,tmpx,tmpy);
for(i=1;i<=PolyGonA.nNode+PolyGonB.nNode;i++)
{
sleep(1);
tmpx=trace[i].x;
tmpy=N-trace[i].y;
LineTo(hDC,tmpx,tmpy);
Sleep(500);
}
}
LRESULT CALLBACK WindowProcedure (HWND, UINT, WPARAM, LPARAM);
char szClassName[ ] = " NFP 算法 " ;
int flag = 0 ;
int WINAPI WinMain (HINSTANCE hThisInstance, HINSTANCE hPrevInstance,
LPSTR lpszArgument, int nFunsterStil)
{
HWND hwnd; /* This is the handle for our window */
MSG messages; /* Here messages to the application are saved */
WNDCLASSEX wincl; /* Data structure for the windowclass */
wincl.hInstance = hThisInstance;
wincl.lpszClassName = szClassName;
wincl.lpfnWndProc = WindowProcedure; /* This function is called by windows */
wincl.style = CS_DBLCLKS; /* Catch double-clicks */
wincl.cbSize = sizeof (WNDCLASSEX);
wincl.hIcon = LoadIcon (NULL, IDI_APPLICATION);
wincl.hIconSm = LoadIcon (NULL, IDI_APPLICATION);
wincl.hCursor = LoadCursor (NULL, IDC_ARROW);
wincl.lpszMenuName = NULL; /* No menu */
wincl.cbClsExtra = 0; /* No extra bytes after the window class */
wincl.cbWndExtra = 0; /* structure or the window instance */
wincl.hbrBackground = (HBRUSH) COLOR_BACKGROUND;
if (!RegisterClassEx (&wincl))
return 0;
hwnd = CreateWindowEx (
0, /* Extended possibilites for variation */
szClassName, /* Classname */
"NFP 算法", /* Title Text */
WS_OVERLAPPEDWINDOW, /* default window */
0, /* Windows decides the position */
0, /* where the window ends up on the screen */
W, /* The programs width */
H, /* and height in pixels */
HWND_DESKTOP, /* The window is a child-window to desktop */
NULL, /* No menu */
hThisInstance, /* Program Instance handler */
NULL /* No Window Creation data */
);
ShowWindow(hwnd,nFunsterStil);
while (GetMessage(&messages, NULL, 0, 0))
{
TranslateMessage(&messages);
DispatchMessage(&messages);
}
return messages.wParam;
}
LRESULT CALLBACK WindowProcedure (HWND hwnd, UINT message, WPARAM wParam, LPARAM lParam)
{
HDC hDC;
PAINTSTRUCT PtStr;
static HWND hwndButton = 0;
switch (message) /* handle the messages */
{
case WM_CREATE:
hwndButton = CreateWindow (
"button",/* Builtin button class */
"Click Here",
WS_CHILD | WS_VISIBLE | BS_PUSHBUTTON,
0, 0, 100, 50,
hwnd,/* Parent is this window. */
(HMENU) 1,/* Control ID: 1 */
((LPCREATESTRUCT) lParam)->hInstance,
NULL
);
break;
case WM_PAINT:
/*********************************************************/
break;
case WM_COMMAND:
if (flag==1)
break;
else
flag=1;
/* Check the control ID, notification code and
* control handle to see if this is a button click
* message from our child button. */
if (LOWORD(wParam) == 1 &&
HIWORD(wParam) == BN_CLICKED &&
(HWND) lParam == hwndButton)
{
/* Our button was clicked. Close the window. */
hDC=BeginPaint(hwnd,&PtStr);
HPEN hPen=CreatePen(PS_SOLID,1,RGB(255,0,0));
SelectObject(hDC,hPen);
/*********************************************************/
init();
CreatEdgeList();
DrawPolyGon(hDC,PolyGonA.Edge,PolyGonA.nNode);
hPen=CreatePen(PS_SOLID,1,RGB(255,255,0));
SelectObject(hDC,hPen);
DrawPolyGon(hDC,PolyGonB.Edge,PolyGonB.nNode);
Sleep(5000);
Adjust();
hPen=CreatePen(PS_SOLID,1,RGB(255,0,0));
SelectObject(hDC,hPen);
DrawPolyGon(hDC,PolyGonA.Edge,PolyGonA.nNode);
TurnPolyGon(hDC);
hPen=CreatePen(PS_SOLID,1,RGB(192,192,192));
SelectObject(hDC,hPen);
DrawPolyGon(hDC,PolyGonB.Edge,PolyGonB.nNode);
EndPaint(hwnd,&PtStr);
}
flag=0;
break;
case WM_DESTROY:
PostQuitMessage (0); /* send a WM_QUIT to the message queue */
break;
default: /* for messages that we don't deal with */
return DefWindowProc (hwnd, message, wParam, lParam);
}
return 0;
}
|
|