最近我写了一个关于凸多边形碰撞的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;
}
