計算幾何幾何函數庫

 計算幾何幾何函數庫
-------------------------------------------------------------------------------------------------------------------------------
導引
1. 常量定義和包含文件
2. 基本數據結構
3. 精度控制
㈠ 點的基本運算
1. 平面上兩點之間距離
2. 判斷兩點是否重合
3. 矢量叉乘
4. 矢量點乘
5. 判斷點是否在線段上
6. 求一點饒某點旋轉后的坐標
7. 求矢量夾角
㈡ 線段及直線的基本運算
1. 點與線段的關系
2. 求點到線段所在直線垂線的垂足
3. 點到線段的最近點
4. 點到線段所在直線的距離
5. 點到折線集的最近距離
6. 判斷圓是否在多邊形內
7. 求矢量夾角余弦
8. 求線段之間的夾角
9. 判斷線段是否相交
10.判斷線段是否相交但不交在端點處
11.求點關於某直線的對稱點
12.判斷兩條直線是否相交及求直線交點
13.判斷線段是否相交，如果相交返回交點
㈢ 多邊形常用算法模塊
1. 判斷多邊形是否簡單多邊形
2. 檢查多邊形頂點的凸凹性
3. 判斷多邊形是否凸多邊形
4. 求多邊形面積
5. 判斷多邊形頂點的排列方向
7. 射線法判斷點是否在多邊形內
8. 判斷點是否在凸多邊形內
9. 尋找點集的graham算法
10.尋找點集凸包的卷包裹法
11.凸包MelkMan算法的實現
12. 凸多邊形的直徑
13.求凸多邊形的重心
===========================================================================
導引
/* 需要包含的頭文件 */
#include <cmath >
/* 常量定義 */
const double INF = 1E200;
const double EP = 1E-10;
const int MAXV = 300;
const double PI = 3.14159265;
/* 基本幾何結構 */
struct POINT
{
double x;
double y;
POINT(double a=0, double b=0) { x=a; y=b;}
};
struct LINESEG
{
POINT s;
POINT e;
LINESEG(POINT a, POINT b) { s=a; e=b;}
LINESEG() { }
};
// 直線的解析方程 a*x+b*y+c=0 為統一表示，約定 a>= 0
struct LINE
{
double a;
double b;
double c;
LINE(double d1=1, double d2=-1, double d3=0) {a=d1; b=d2; c=d3;}
};
//線段樹
struct LINETREE
{
}
//浮點誤差的處理
int dblcmp(double d)
{
if(fabs(d)<EP)
return 0 ;
return (d>0) ?1 :-1 ;
}
<一>點的基本運算
// 返回兩點之間歐氏距離
double dist(POINT p1,POINT p2)
{
return( sqrt( (p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y) ) );
}
// 判斷兩個點是否重合
bool equal_point(POINT p1,POINT p2)
{
return ( (abs(p1.x-p2.x)<EP)&&(abs(p1.y-p2.y)<EP) );
}
/*(sp-op)*(ep-op)的叉積
r=multiply(sp,ep,op),得到(sp-op)*(ep-op)的叉積
r>0:sp在矢量op ep的順時針方向；
r=0：op sp ep三點共線；
r<0: sp在矢量op ep的逆時針方向 */
double multiply(POINT sp,POINT ep,POINT op)
{
return((sp.x-op.x)*(ep.y-op.y) - (ep.x-op.x)*(sp.y-op.y));
}
double amultiply(POINT sp,POINT ep,POINT op)
{
return fabs((sp.x-op.x)*(ep.y-op.y)-(ep.x-op.x)*(sp.y-op.y));
}
/*矢量(p1-op)和(p2-op)的點積
r=dotmultiply(p1,p2,op),得到矢量(p1-op)和(p2-op)的點積如果兩個矢量都非零矢量
r < 0: 兩矢量夾角為銳角；
r = 0：兩矢量夾角為直角；
r > 0: 兩矢量夾角為鈍角 */
double dotmultiply(POINT p1,POINT p2,POINT p0)
{
return ((p1.x-p0.x)*(p2.x-p0.x) + (p1.y-p0.y)*(p2.y-p0.y));
}
/* 判斷點p是否在線段l上
條件：(p在線段l所在的直線上)&& (點p在以線段l為對角線的矩形內) */
bool online(LINESEG l,POINT p)
{
return ((multiply(l.e，p，l.s)==0)
&& ( ( (p.x-l.s.x) * (p.x-l.e.x) <=0 ) && ( (p.y-l.s.y)*(p.y-l.e.y) <=0 ) ) );
}
// 返回點p以點o為圓心逆時針旋轉alpha(單位：弧度)后所在的位置
POINT rotate(POINT o，double alpha，POINT p)
{
POINT tp;
p.x -=o.x;
p.y -=o.y;
tp.x=p.x*cos(alpha) - p.y*sin(alpha)+o.x;
tp.y=p.y*cos(alpha) + p.x*sin(alpha)+o.y;
return tp;
}
/* 返回頂角在o點，起始邊為os，終止邊為oe的夾角(單位：弧度)
角度小於pi，返回正值
角度大於pi，返回負值
可以用於求線段之間的夾角 */
double angle(POINT o,POINT s,POINT e)
{
double cosfi,fi,norm;
double dsx = s.x - o.x;
double dsy = s.y - o.y;
double dex = e.x - o.x;
double dey = e.y - o.y;
cosfi=dsx*dex+dsy*dey;
norm=(dsx*dsx+dey*dey)*(dex*dex+dey*dey);
cosfi /= sqrt( norm );
if (cosfi >= 1.0 ) return 0;
if (cosfi <= -1.0 ) return -3.1415926;
fi=acos(cosfi);
if (dsx*dey-dsy*dex>0) return fi;// 說明矢量os 在矢量 oe的順時針方向
return -fi;
}
<二>線段及直線的基本運算
/* 判斷點C在線段AB所在的直線l上垂足P的與線段AB的關系
本函數是根據下面的公式寫的，P是點C到線段AB所在直線的垂足
AC dot AB
r = ----------------------
||AB||^2
(Cx-Ax)(Bx-Ax) + (Cy-Ay)(By-Ay)
= ----------------------------------------------------
L^2
r has the following meaning:
r=0 P = A
r=1 P = B
r<0 P is on the backward extension of AB
r>1 P is on the forward extension of AB
0<r<1 P is interior to AB
*/
double relation(POINT c,LINESEG l)
{
LINESEG tl;
tl.s=l.s;
tl.e=c;
return dotmultiply(tl.e,l.e,l.s)/(dist(l.s,l.e)*dist(l.s,l.e));
}
// 求點C到線段AB所在直線的垂足 P
POINT perpendicular(POINT p,LINESEG l)
{
double r=relation(p,l);
POINT tp;
tp.x=l.s.x+r*(l.e.x-l.s.x);
tp.y=l.s.y+r*(l.e.y-l.s.y);
return tp;
}
/* 求點p到線段l的最短距離
返回線段上距該點最近的點np 注意：np是線段l上到點p最近的點，不一定是垂足 */
double ptolinesegdist(POINT p,LINESEG l,POINT &np)
{
double r=relation(p,l);
if(r<0)
{
np=l.s;
return dist(p,l.s);
}
if(r>1)
{
np=l.e;
return dist(p,l.e);
}
np=perpendicular(p,l);
return dist(p,np);
}
// 求點p到線段l所在直線的距離
//請注意本函數與上個函數的區別
double ptoldist(POINT p,LINESEG l)
{
return abs(multiply(p,l.e,l.s))/dist(l.s,l.e);
}
/* 計算點到折線集的最近距離,並返回最近點.
注意：調用的是ptolineseg()函數 */
double ptopointset(int vcount, POINT pointset[], POINT p, POINT &q)
{
int i;
double cd=double(INF),td;
LINESEG l;
POINT tq,cq;
for(i=0;i<vcount-1;i++)
{
l.s=pointset[i];
l.e=pointset[i+1];
td=ptolinesegdist(p,l,tq);
if(td<cd)
{
cd=td;
cq=tq;
}
}
q=cq;
return cd;
}
/* 判斷圓是否在多邊形內*/
bool CircleInsidePolygon(int vcount,POINT center,double radius,POINT polygon[])
{
POINT q;
double d;
q.x=0;
q.y=0;
d=ptopointset(vcount,polygon,center,q);
if(d<radius||fabs(d-radius)<EP) return true;
else return false;
}
/* 返回兩個矢量l1和l2的夾角的余弦 (-1 ~ 1)
注意：如果想從余弦求夾角的話，注意反余弦函數的值域是從 0到pi */
double cosine(LINESEG l1,LINESEG l2)
{
return(((l1.e.x-l1.s.x)*(l2.e.x-l2.s.x)+(l1.e.y-l1.s.y)*(l2.e.y-l2.s.y))/(dist(l1.e,l1.s)*dist(l2.e,l2.s))) );
}
// 返回線段l1與l2之間的夾角
//單位：弧度 范圍(-pi，pi)
double lsangle(LINESEG l1,LINESEG l2)
{
POINT o,s,e;
o.x=o.y=0;
s.x=l1.e.x-l1.s.x;
s.y=l1.e.y-l1.s.y;
e.x=l2.e.x-l2.s.x;
e.y=l2.e.y-l2.s.y;
return angle(o,s,e);
}
//判斷線段u和v相交(包括相交在端點處)
bool intersect(LINESEG u,LINESEG v)
{
return ( (max(u.s.x,u.e.x)>=min(v.s.x,v.e.x))&& //排斥實驗
(max(v.s.x,v.e.x)>=min(u.s.x,u.e.x))&&
(max(u.s.y,u.e.y)>=min(v.s.y,v.e.y))&&
(max(v.s.y,v.e.y)>=min(u.s.y,u.e.y))&&
(multiply(v.s,u.e,u.s)*multiply(u.e,v.e,u.s)>=0)&& //跨立實驗
(multiply(u.s,v.e,v.s)*multiply(v.e,u.e,v.s)>=0));
}
// 判斷線段u和v相交（不包括雙方的端點）
bool intersect_A(LINESEG u,LINESEG v)
{
return ((intersect(u,v)) &&
(!online(u,v.s)) &&
(!online(u,v.e)) &&
(!online(v,u.e)) &&
(!online(v,u.s)));
}
// 判斷線段v所在直線與線段u相交
方法：判斷線段u是否跨立線段v
bool intersect_l(LINESEG u,LINESEG v)
{
return multiply(u.s,v.e,v.s)*multiply(v.e,u.e,v.s)>=0;
}
// 根據已知兩點坐標，求過這兩點的直線解析方程： a*x+b*y+c = 0 (a >= 0)
LINE makeline(POINT p1,POINT p2)
{
LINE tl;
int sign = 1;
tl.a=p2.y-p1.y;
if(tl.a<0)
{
sign = -1;
tl.a=sign*tl.a;
}
tl.b=sign*(p1.x-p2.x);
tl.c=sign*(p1.y*p2.x-p1.x*p2.y);
return tl;
}
// 根據直線解析方程返回直線的斜率k,水平線返回 0,豎直線返回 1e200
double slope(LINE l)
{
if(abs(l.a) < 1e-20)return 0;
if(abs(l.b) < 1e-20)return INF;
return -(l.a/l.b);
}
// 返回直線的傾斜角alpha ( 0 - pi)
// 注意：atan()返回的是 -PI/2 ~ PI/2
double alpha(LINE l)
{
if(abs(l.a)< EP)return 0;
if(abs(l.b)< EP)return PI/2;
double k=slope(l);
if(k>0)
return atan(k);
else
return PI+atan(k);
}
// 求點p關於直線l的對稱點
POINT symmetry(LINE l,POINT p)
{
POINT tp;
tp.x=((l.b*l.b-l.a*l.a)*p.x-2*l.a*l.b*p.y-2*l.a*l.c)/(l.a*l.a+l.b*l.b);
tp.y=((l.a*l.a-l.b*l.b)*p.y-2*l.a*l.b*p.x-2*l.b*l.c)/(l.a*l.a+l.b*l.b);
return tp;
}
// 如果兩條直線 l1(a1*x+b1*y+c1 = 0), l2(a2*x+b2*y+c2 = 0)相交，返回true，且返回交點p
bool lineintersect(LINE l1,LINE l2,POINT &p) // 是 L1，L2
{
double d=l1.a*l2.b-l2.a*l1.b;
if(abs(d)<EP) // 不相交
return false;
p.x = (l2.c*l1.b-l1.c*l2.b)/d;
p.y = (l2.a*l1.c-l1.a*l2.c)/d;
return true;
}
// 如果線段l1和l2相交，返回true且交點由(inter)返回，否則返回false
bool intersection(LINESEG l1,LINESEG l2,POINT &inter)
{
LINE ll1,ll2;
ll1=makeline(l1.s,l1.e);
ll2=makeline(l2.s,l2.e);
if(lineintersect(ll1,ll2,inter)) return online(l1,inter);
else return false;
}
<三> 多邊形常用算法模塊
如果無特別說明，輸入多邊形頂點要求按逆時針排列
// 返回多邊形面積(signed)；
// 輸入頂點按逆時針排列時，返回正值；否則返回負值
double area_of_polygon(int vcount,POINT polygon[])
{
int i;
double s;
if (vcount<3)
return 0;
s=polygon[0].y*(polygon[vcount-1].x-polygon[1].x);
for (i=1;i<vcount;i++)
s+=polygon[i].y*(polygon[(i-1)].x-polygon[(i+1)%vcount].x);
return s/2;
}
// 判斷頂點是否按逆時針排列
// 如果輸入頂點按逆時針排列，返回true
bool isconterclock(int vcount,POINT polygon[])
{
return area_of_polygon(vcount,polygon)>0;
}
/*射線法判斷點q與多邊形polygon的位置關系
要求polygon為簡單多邊形，頂點時針排列
如果點在多邊形內： 返回0
如果點在多邊形邊上：返回1
如果點在多邊形外： 返回2 */
int insidepolygon(POINT q)
{
int c=0,i,n;
LINESEG l1,l2;
l1.s=q; l1.e=q;l1.e.x=double(INF);
n=vcount;
for (i=0;i<vcount;i++)
{
l2.s=Polygon[i];
l2.e=Polygon[(i+1)%vcount];
double ee= Polygon[(i+2)%vcount].x;
double ss= Polygon[(i+3)%vcount].y;
if(online(l2,q))
return 1;
if(intersect_A(l1,l2))
c++; // 相交且不在端點
if(online(l1,l2.e)&& !online(l1,l2.s) && l2.e.y>l2.e.y)
c++;//l2的一個端點在l1上且該端點是兩端點中縱坐標較大的那個
if(!online(l1,l2.e)&& online(l1,l2.s) && l2.e.y<l2.e.y)
c++;//忽略平行邊
}
if(c%2 == 1)
return 0;
else
return 2;
}
//判斷點q在凸多邊形polygon內
// 點q是凸多邊形polygon內[包括邊上]時，返回true
// 注意：多邊形polygon一定要是凸多邊形
bool InsideConvexPolygon(int vcount,POINT polygon[],POINT q)
{
POINT p;
LINESEG l;
int i;
p.x=0; p.y=0;
for(i=0;i<vcount;i++) // 尋找一個肯定在多邊形polygon內的點p：多邊形頂點平均值
{
p.x+=polygon[i].x;
p.y+=polygon[i].y;
}
p.x /= vcount;
p.y /= vcount;
for(i=0;i<vcount;i++)
{
l.s=polygon[i];
l.e=polygon[(i+1)%vcount];
if(multiply(p,l.e,l.s)*multiply(q,l.e,l.s)<0)
/* 點p和點q在邊l的兩側，說明點q肯定在多邊形外 */
return false；
}
return true；
}
/*尋找凸包的graham 掃描法
PointSet為輸入的點集；
ch為輸出的凸包上的點集，按照逆時針方向排列;
n為PointSet中的點的數目
len為輸出的凸包上的點的個數 */
void Graham_scan(POINT PointSet[],POINT ch[],int n,int &len)
{
int i,j,k=0,top=2;
POINT tmp;
// 選取PointSet中y坐標最小的點PointSet[k]，如果這樣的點有多個，則取最左邊的一個
for(i=1;i<n;i++)
if ( PointSet[i].y<PointSet[k].y || (PointSet[i].y==PointSet[k].y)
&& (PointSet[i].x<PointSet[k].x) )
k=i;
tmp=PointSet[0];
PointSet[0]=PointSet[k];
PointSet[k]=tmp; // 現在PointSet中y坐標最小的點在PointSet[0]
for (i=1;i<n-1;i++) /* 對頂點按照相對PointSet[0]的極角從小到大進行排序，極角相同
的按照距離PointSet[0]從近到遠進行排序 */
{
k=i;
for (j=i+1;j<n;j++)
if ( multiply(PointSet[j],PointSet[k],PointSet[0])>0 || // 極角更小
(multiply(PointSet[j],PointSet[k],PointSet[0])==0) && /*極角相等，距離更短 */ dist(PointSet[0],PointSet[j])<dist(PointSet[0],PointSet[k]) )
k=j;
tmp=PointSet[i];
PointSet[i]=PointSet[k];
PointSet[k]=tmp;
}
ch[0]=PointSet[0];
ch[1]=PointSet[1];
ch[2]=PointSet[2];
for (i=3;i<n;i++)
{
while (multiply(PointSet[i],ch[top],ch[top-1])>=0) top--;
ch[++top]=PointSet[i];
}
len=top+1;
}
// 卷包裹法求點集凸殼，參數說明同graham算法
void ConvexClosure(POINT PointSet[],POINT ch[],int n,int &len)
{
int top=0,i,index,first;
double curmax,curcos,curdis;
POINT tmp;
LINESEG l1,l2;
bool use[MAXV];
tmp=PointSet[0];
index=0;
// 選取y最小點，如果多於一個，則選取最左點
for(i=1;i<n;i++)
{
if(PointSet[i].y<tmp.y||PointSet[i].y == tmp.y&&PointSet[i].x<tmp.x)
{
index=i;
}
use[i]=false;
}
tmp=PointSet[index];
first=index;
use[index]=true;
index=-1;
ch[top++]=tmp;
tmp.x-=100;
l1.s=tmp;
l1.e=ch[0];
l2.s=ch[0];
while(index!=first)
{
curmax=-100;
curdis=0;
// 選取與最后一條確定邊夾角最小的點，即余弦值最大者
for(i=0;i<n;i++)
{
if(use[i])continue;
l2.e=PointSet[i];
curcos=cosine(l1,l2); // 根據cos值求夾角余弦，范圍在 （-1 -- 1 ）
if(curcos>curmax || fabs(curcos-curmax)<1e-6 && dist(l2.s,l2.e)>curdis)
{
curmax=curcos;
index=i;
curdis=dist(l2.s,l2.e);
}
}
use[first]=false; //清空第first個頂點標志，使最后能形成封閉的hull
use[index]=true;
ch[top++]=PointSet[index];
l1.s=ch[top-2];
l1.e=ch[top-1];
l2.s=ch[top-1];
}
len=top-1;
}
// 求凸多邊形的重心,要求輸入多邊形按逆時針排序
POINT gravitycenter(int vcount,POINT polygon[])
{
POINT tp;
double x,y,s,x0,y0,cs,k;
x=0;y=0;s=0;
for(int i=1;i<vcount-1;i++)
{
x0=(polygon[0].x+polygon[i].x+polygon[i+1].x)/3;
y0=(polygon[0].y+polygon[i].y+polygon[i+1].y)/3; //求當前三角形的重心
cs=multiply(polygon[i],polygon[i+1],polygon[0])/2;
//三角形面積可以直接利用該公式求解
if(abs(s)<1e-20)
{
x=x0;y=y0;s+=cs;continue;
}
k=cs/s; //求面積比例
x=(x+k*x0)/(1+k);
y=(y+k*y0)/(1+k);
s += cs;
}
tp.x=x;
tp.y=y;
return tp;
}
/*所謂凸多邊形的直徑，即凸多邊形任兩個頂點的最大距離。下面的算法
僅耗時O(n)，是一個優秀的算法。 輸入必須是一個凸多邊形，且頂點
必須按順序（順時針、逆時針均可）依次輸入。若輸入不是凸多邊形
而是一般點集，則要先求其凸包。 就是先求出所有跖對，然后求出每
個跖對的距離，取最大者。點數要多於5個*/
void Diameter(POINT ch[],int n,double &dia)
{
int znum=0,i,j,k=1;
int zd[MAXV][2];
double tmp;
while(amultiply(ch[0],ch[k+1],ch[n-1]) > amultiply(ch[0],ch[k],ch[n-1])-EP)
k++;
i=0;
j=k;
while(i<=k && j<n)
{
zd[znum][0]=i;
zd[znum++][1]=j;
while(amultiply(ch[i+1],ch[j+1],ch[i]) > amultiply(ch[i+1],ch[j],ch[i]) – EP
&& j< n-1)
{
zd[znum][0]=i;
zd[znum++][1]=j;
j++;
}
i++;
}
dia=-1.0;
for(i=0;i<znum;i++)
{
printf("%d %d/n",zd[i][0],zd[i][1]);
tmp=dist(ch[zd[i][0]],ch[zd[i][1]]);
if(dia<tmp)
dia=tmp;
}
}