最近我写了一个关于凸多边形碰撞的NFP算法的小程序,这里把代码传上来大家看一看。程序是用devc++实现的windowsAPI程序。
http://download1.youkuaiyun.com/down3/20070622/22130938822.rar
代码如下:
#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;
}

本文介绍了一个用于凸多边形碰撞检测的NFP算法,并分享了使用DevC++实现的Windows API程序源代码。该算法通过计算多边形的形心、排序顶点并调整位置来实现两个多边形的碰撞检测。
1271

被折叠的 条评论
为什么被折叠?



