geometrylib.cpp

国内的一个A*算法演示

#include "stdafx.h"
#include "geometrylib.h"

#include <math.h>

//保存调节点的相关值
extern int g_nAdjArc;
extern int g_nAdjLen;
extern int g_nAdjLenBig;

const double INFINITY=-200000;

/********************************************/
/*               两点间距离                 */
/********************************************/
float dis_PP(float x1,float y1,float x2,float y2)
{
	return sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) );
}
float dis_PP(POINT a,POINT b)
{
	return sqrt((float)(a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
}
/********************************************/
/*             判断两点是否共点             */
/********************************************/
bool equal_PP(POINT a,POINT b)
{    
	if( a.x==b.x && a.y==b.y )
		return true;
	else
		return false;
}

/********************************************/
/* 返回(P1-P0)*(P2-P0)的叉积。              */
/* 若为正，则<P0,P1>在<P0,P2>的顺时针方向   */
/* 若为零，则<P0,P1><P0,P2>共线；           */
/* 若为负，则<P0,P1>在<P0,P2>的在逆时针方向 */
/* 这个函数可以确定两条线段在交点处的转向   */
/* 比如确定p0p1和p1p2在p1处是左转还是右转， */
/* 只要求 (p2-p0)*(p1-p0)，                 */
/* 若<0则左转，>0则右转，=0则共线           */
/********************************************/
float multiply( POINT p1 , POINT p2 , POINT p0 )
{     
	////应用平方根是为了加大误差值
	//float ajk=(p1.x-p0.x)*(p2.y-p0.y);
	//ajk=sqrt(fabs(ajk));
	//float bjk=(p2.x-p0.x)*(p1.y-p0.y);
	//bjk=sqrt(fabs(bjk));
	//int abjk=ajk-bjk;
	return ((p1.x-p0.x)*(p2.y-p0.y)-(p2.x-p0.x)*(p1.y-p0.y));
}
/********************************************/
/*           判断点p是否在线段ab上          */
/********************************************/
bool online( POINT a,POINT b, POINT p )
{   
	//if( equal_PP(p,a) || equal_PP(p,b) )
	//	return true;
	//int aj=multiply(b,p,a);
	//bool ak=min(a.x,b.x)<p.x<max(a.x,b.x);
	//bool al=min(a.y,b.y)<p.y<max(a.y,b.y);
	return( (multiply(b,p,a)==0)&&( (min(a.x,b.x)<=p.x&&p.x<=max(a.x,b.x) )&&( min(a.y,b.y)<=p.y&&p.y<=max(a.y,b.y))) );
}
/************************************************************************/
/* 计算点P到直线AB的垂足
/************************************************************************/
POINT dis_CZ(POINT p,POINT a,POINT b)
{
	if(online(a,b,p))
	{
		return p;
	}
	POINT d;//垂足
	float dx,dy;//垂足
	float ax=a.x;
	float ay=a.y;
	float bx=b.x;
	float by=b.y;
	float px=p.x;
	float py=p.y;
	if(a.x==b.x)
	{
		d.x=a.x;
		d.y=p.y;
		dx=d.x;
		dy=d.y;
	}
	else if(a.y==b.y)
	{
		d.x=p.x;
		d.y=a.y;
		dx=d.x;
		dy=d.y;
	}
	else 
	{
		float k;//斜率
		k=(by-ay)/(bx-ax);
		dx=(k*k*ax+k*(py-ay)+px)/(k*k+1); 
		dy=k*(dx-ax)+ay;
		d.x=dx;
		d.y=dy;
		int x=dx*10;
		int y=dy*10;
		if(x%10>4)
			++d.x;
		if(y%10>4)
			++d.y;
	}
	return d;
}
/************************************************************************/
/* 计算点P到线段AB的距离 
设线段的两端点为pt1和pt2，斜率为：k = ( pt2.y - pt1. y ) / (pt2.x - pt1.x );
该直线方程为：y = k* ( x - pt1.x) + pt1.y。
其垂线的斜率为 - 1 / k，垂线方程为：y = (-1/k) * (x - point.x) + point.y 。 
　　联立两直线方程解得：
  x = ( k^2 * pt1.x + k * (point.y - pt1.y ) + point.x ) / ( k^2 + 1) ，
  y = k * ( x - pt1.x) + pt1.y;
  然后再判断垂足是否在线段上，如果在线段上则返回P到垂足的距离；
  如果不在则计算两端点到垂足的距离，选择距离较近的返回。
/************************************************************************/
float dis_PL(POINT p,POINT a,POINT b)
{
	if(online(a,b,p))
		return 0.0;
	POINT d;//垂足
	d=dis_CZ(p,a,b);
	//如果点在线段AB上，应该用ONLINE但精度还是高，所以此处简化，判断D是否在AB所形成的矩形内
	if((min(a.x,b.x)<=d.x&&d.x<=max(a.x,b.x) )||( min(a.y,b.y)<=d.y&&d.y<=max(a.y,b.y)))//online(a,b,d))
	{
		return dis_PP(p.x,p.y,d.x,d.y);
	}
	else
	{
		
		return min(dis_PP(a.x,a.y,d.x,d.y),dis_PP(b.x,b.y,d.x,d.y));
	}
}
/************************************************************************/
/* 计算点P到AB所在直线的距离 
  /************************************************************************/
float dis_PBL(POINT p,POINT a,POINT b)
{
	if(online(a,b,p))
		return 0.0;
	POINT d;//垂足
	d=dis_CZ(p,a,b);
	return dis_PP(p.x,p.y,d.x,d.y);
}
/********************************************/
/*             判断直线段相交               */
/********************************************/
bool inter_SS( POINT a1,POINT a2,POINT b1,POINT b2)
{ 
	return( (max(a1.x,a2.x)>=min(b1.x,b2.x))&&
		(max(b1.x,b2.x)>=min(a1.x,a2.x))&&
		(max(a1.y,a2.y)>=min(b1.y,b2.y))&&
		(max(b1.y,b2.y)>=min(a1.y,a2.y))&&
		(multiply(b1,a2,a1)*multiply(a2,b2,a1)>=0)&&
		(multiply(a1,b2,b1)*multiply(b2,a2,b1)>=0));
}
/********************************************/
/*              判断点是否在圆内            */
/********************************************/
bool insideRound( POINT o,int r ,POINT p )
{     
	return ( dis_PP(p.x,p.y,o.x,o.y)<=r );
}
/************************************************************************/
/* 判断线段与圆是否相交 
   先计算圆心到线段的最短距离如果大于半径则不线段AB和圆R不相交
/************************************************************************/
bool inter_SR(POINT a,POINT b,POINT o,int r)
{
	//a,b都包含在圆O内，则没有交点
	//if(insideRound(o,r,a)&&insideRound(o,r,b))
	//	return false;
	if(insideRound(o,r,a)||insideRound(o,r,b))
		return true;
	float k= dis_PL(o,a,b);
	if(r>=k)
		return true;
	else
		return false;
}
/************************************************************************/
/* 求线段AB在延长线B点以外,从A开始N个长度的点P                          */
/************************************************************************/
POINT LineProlong(POINT a,POINT b,float fLen)
{
	POINT p;
	if(a.x==b.x)//平行于Y轴
	{
		if(a.y>b.y)
		{
			p.y=b.y-fLen;
		}
		else
		{
			p.y=b.y+fLen;
		}
		p.x=a.x;
		return p;
	}
	if(a.y==b.y)
	{
		if(a.x>b.x)
		{
			p.x=b.x-fLen;
		}
		else
		{
			p.x=b.x+fLen;
		}
		p.y=a.y;
		return p;
	}
	float ax=a.x;
	float ay=a.y;
	float bx=b.x;
	float by=b.y;
	float ba=(ay-by)/(ax-bx);
	//求点B在直线BD上左右一个单位的两个点M,N的坐标
	float fGen=sqrt(ba*ba+1);
	float fGen1=fGen*-1;
	float mx,my,nx,ny;
	mx=fLen/fGen+bx;
	my=ba*(mx-bx)+by;
	nx=fLen/fGen1+bx;
	ny=ba*(nx-bx)+by;
	//求远离D点的坐标
	if(dis_PP(mx,my,ax,ay)>dis_PP(nx,ny,ax,ay))
	{
		p.x=mx;
		p.y=my;
		return p;
	}
	else
	{
		p.x=nx;
		p.y=ny;
		return p;
	}
}
/************************************************************************/
/* 如果寻路的点同时在圆上和多边形顶点上，则取此顶点的角平分线的反方向一个单位
   传入的为角ABC，B为角所在点
/************************************************************************/
POINT adjust(POINT a, POINT b,POINT c)
{
	POINT Yes;
	POINT aa=LineProlong(b,a,200.0);
	POINT bb=LineProlong(b,c,200.0);
	POINT cc;
	cc.x=(aa.x+bb.x)/2;
	cc.y=(aa.y+bb.y)/2; 
	//当角度小于50度时，调节点与B点距离加大30:103.5  45:153.1 
	//公式是 2*200.0*sin(aa,bb的角度) 这里设为40
	int nLimit=2*200.0*sin((double)g_nAdjArc*3.14159265358979323846/180);
	int aabb=dis_PP(aa,bb);
	if(aabb<nLimit)
		Yes=LineProlong(cc,b,g_nAdjLenBig);
	else
        Yes=LineProlong(cc,b,g_nAdjLen);//6是调节点，
	return Yes;
}
/********************************************/
/*          判断点是否在多边形内            */
/********************************************/
bool insidePolygon( int vcount , POINT ver[] , POINT point )
{    
	int i,c=0;
	POINT t;
	POINT lineA,lineB,edgeA,edgeB;
	t=ver[vcount];
	ver[vcount]=ver[0];
	lineA=lineB=point;
	lineB.x=INFINITY;
	for(i=0;i<vcount;i++)
	{
		edgeA=ver[i]; 
		edgeB=ver[i+1];
		if( online(edgeA,edgeB,point) )
			return true;
		if( edgeA.y==edgeB.y )
			continue;
		if( (min(edgeA.y,edgeB.y)!=point.y)&&inter_SS(lineA,lineB,edgeA,edgeB))
			c++;
	}
	ver[vcount]=t;
	return c%2 ;
}