点、线、弧相关算法

1、计算两点间距离、

2、求线段夹角(弧度)、已知圆上三点求圆心半径

3、点p绕o逆时针旋转alpha（弧度）、已知圆弧上三点求圆弧时针方向

4、求线段交点，必须存在交点，或者预判断【平面几何方法】

5、求点p到线段l的最短距离,并返回线段上距该点最近的点np

6、求点p到直线l的最短距离

7、判点在线段上

8、求ab是否与cd相交，交点为p。1规范相交，0非共线非端点重合的非规范相交 -1端点重合的非规范相交 -2共线的非规范相交 -3不相交

9、判点与线段所在直线方位关系 0在直线上 -1在直线左 2在直线右

10、判断点到直线垂足与线段的关系

11、三角形重心、多边形重心、判点在多边形内部、比较两个向量pt1和pt2分别与x轴向量(1, 0)的夹角

12、贝塞尔曲线离散

源码.hpp

可将其设置为动态库，直接调用

#pragma once
#include "stdafx.h"

#ifndef MATH_PI
#define MATH_PI	(3.14159265358979323846264338327950)
#endif


#define max(a,b) (((a)>(b))?(a):(b))
#define min(a,b) (((a)>(b))?(b):(a))
#define sign(x) ((x)>eps?1:((x)<-eps?(-1):(0))) 
const double eps = 1e-8, inf = 1e50, Pi = acos(-1.0);

template<typename T>
struct line
{
	T a, b;
	line();
	line(T _a, T _b)
	{
		a.x = _a.x;
		a.y = _a.y;
		b.x = _b.x;
		b.y = _b.y;
	}
	line(T _a, T _b, int scale/*缩小比例*/)
	{
		a.x = _a.x / scale;
		a.y = _a.y / scale;
		b.x = _b.x / scale;
		b.y = _b.y / scale;
	}
};

template<typename T>
inline double xmult(T a, T b) {
	return a.x*b.y - a.y*b.x;
}

template<typename T>
inline double xmult(T o, T a, T b)
{
	return (a.x - o.x)*(b.y - o.y) - (b.x - o.x)*(a.y - o.y);
}

template<typename T>
inline double dmult(T a, T b)
{
	return a.x*b.x + a.y*b.y;
}

template<typename T>
inline double lenth(T a){
    return sqrt(dmult(a,a));
}

template<typename T>
inline double dist(T a,T b){
    return lenth(b-a);
}

template<typename T>
inline double dist2(T a,T b){
    return dmult(b-a,b-a);
}

//p在l上的投影与l关系
template<typename T>
double relation(T p,line<T> l) {
    line<T> tl(l.a,p);
    return dmult(tl.b-l.a,l.b-l.a)/dist2(l.a,l.b);
}

//p在l上的垂足
template<typename T>
T perpendicular(T p,line<T> l) {
    double r=relation(p,l);
    return l.a+((l.b-l.a)*r);
}

//oe与os的夹角，夹角正负满足叉积
template<typename T>
double angle(T o, T s, T e) {
	T os, oe;
	os.x = s.x - o.x;
	os.y = s.y - o.y;
	oe.x = e.x - o.x;
	oe.y = e.y - o.y;
	double bot = sqrt(dmult(os, os)*dmult(oe, oe));
	double top = dmult(os, oe);
	double cosfi = top / bot;
	if (cosfi >= 1.0) return 0;
	if (cosfi <= -1.0) return -Pi;
	double fi = acos(cosfi);
	if (xmult(o, s, e) > 0)return fi;
	else return -fi;
}

//计算两点间距离
template<typename T>
double Math_Distance(T pt1, T pt2)
{
	return sqrt(1.0*(pt1.x - pt2.x)*(pt1.x - pt2.x) +
		1.0*(pt1.y - pt2.y)*(pt1.y - pt2.y));
}

//求线段夹角(弧度)
template<typename T>
double Math_2LineAngle(line<T> u, line<T> v) {
	T o, a, b;
	o.x = 0;
	o.y = 0;
	a.x = u.b.x - u.a.x;
	a.y = u.b.y - u.a.y;
	b.x = v.b.x - v.a.x;
	b.y = v.b.y - v.a.y;
	return angle(o, a, b);
}
//点p绕o逆时针旋转alpha（弧度）
template<typename T>
T Math_Rotate(T o, T p, double alpha)
{
	T 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;
}

//求线段交点，必须存在交点，或者预判断【平面几何方法】
template<typename T>
T Math_intersection2(line<T> u, line<T> v)
{
	T ret = u.a;
	double t = xmult(u.a - v.a, v.b - v.a) / xmult(u.b - u.a, v.b - v.a);
	t = fabs(t);
	ret.x += (u.b.x - u.a.x)*t;
	ret.y += (u.b.y - u.a.y)*t;
	return ret;
}

//求点p到线段l的最短距离,并返回线段上距该点最近的点np
template<typename T>
double Math_P2SegDist(T p,line<T> l,T &np) 
{
    double r=relation(p,l);
    if(r<0) {
		np=l.a;
        return dist(p,l.a);
    }
    if(r>1) {
        np=l.b;
        return dist(p,l.b);
    }
    np=perpendicular(p,l);
    return dist(p,np);
}
//求点p到直线l的最短距离
template<typename T>
double Math_P2LineDist(T p,line<T> l) 
{
    return abs(xmult(p-l.a,l.b-l.a))/dist(l.a,l.b);
}

//判点在线段上
template<typename T>
inline bool Math_PonSeg(T a,T p1,T p2) {
    if(fabs(xmult(a,p1,p2))<=eps&&(a.x-p1.x)*(a.x-p2.x)<eps&&(a.y-p1.y)*(a.y-p2.y)<eps)
        return true;
    return false;
}

 //求ab是否与cd相交，交点为p。1规范相交，0非共线非端点重合的非规范相交 -1端点重合的非规范相交 -2共线的非规范相交 -3不相交
template<typename T>
inline int Math_Is2SegInter (T a,T b,T c,T d ,T &p) 
{
    double s1,s2,s3,s4;
    int d1,d2,d3,d4;
    d1=sign(s1=xmult(a,b,c));
    d2=sign(s2=xmult(a,b,d));
    d3=sign(s3=xmult(c,d,a));
    d4=sign(s4=xmult(c,d,b));
	
	//如果规范相交则求交点
    if ((d1^d2)==-2 && (d3^d4)==-2)
    {
        p.x=(c.x*s2-d.x*s1)/(s2-s1);
        p.y=(c.y*s2-d.y*s1)/(s2-s1);
        return 1;
    }
	
	//如果不规范相交 且无重合
    if (d1==0 && Math_PonSeg(c,a,b))
    {
        p=c;
		if (Math_PonSeg(a,c,d) || Math_PonSeg(b,c,d))
		{
			if (((sign(c.x -a.x)==0 && sign(c.y - a.y)==0) ||
				(sign(c.x -b.x)==0 && sign(c.y - b.y)==0)) &&
				!(Math_PonSeg(a,c,d) && Math_PonSeg(b,c,d)))
			{
				return -1;
			}
			return -2;
		}
		else
		{
			if (sign(c.x -a.x)==0 && sign(c.y - a.y)==0)
			{
				return -1;
			}
			if (sign(c.x -b.x)==0 && sign(c.y - b.y)==0)
			{
				return -1;
			}
			return 0;
		}
    }
    if (d2==0 && Math_PonSeg(d,a,b))
    {
        p=d;
		if (Math_PonSeg(a,c,d) || Math_PonSeg(b,c,d))
		{
			if (((sign(d.x -a.x)==0 && sign(d.y - a.y)==0) ||
				(sign(d.x -b.x)==0 && sign(d.y - b.y)==0)) &&
				!(Math_PonSeg(a,c,d) && Math_PonSeg(b,c,d)))
			{
				return -1;
			}
			return -2;
		}
		else
		{
			if (sign(d.x -a.x)==0 && sign(d.y - a.y)==0)
			{
				return -1;
			}
			if (sign(d.x -b.x)==0 && sign(d.y - b.y)==0)
			{
				return -1;
			}
			return 0;
		}
    }
    if (d3==0 && Math_PonSeg(a,c,d))
    {
        p=a;
        if (Math_PonSeg(c,a,b) || Math_PonSeg(d,a,b))
		{
			if (((sign(a.x -c.x)==0 && sign(a.y - c.y)==0) ||
				(sign(a.x -d.x)==0 && sign(a.y - d.y)==0)) &&
				!(Math_PonSeg(c,a,b) && Math_PonSeg(d,a,b)))
			{
				return -1;
			}
			return -2;
		}
		else
		{
			if (sign(a.x -c.x)==0 && sign(a.y - c.y)==0)
			{
				return -1;
			}
			if (sign(a.x -d.x)==0 && sign(a.y - d.y)==0)
			{
				return -1;
			}
			return 0;
		}
    }
    if (d4==0 && Math_PonSeg(b,c,d))
    {
        p=b;
        if (Math_PonSeg(c,a,b) || Math_PonSeg(d,a,b))
		{
			if (((sign(b.x -c.x)==0 && sign(b.y - c.y)==0) ||
				(sign(b.x -d.x)==0 && sign(b.y - d.y)==0)) &&
				!(Math_PonSeg(c,a,b) && Math_PonSeg(d,a,b)))
			{
				return -1;
			}
			return -2;
		}
		else
		{
			if (sign(b.x -c.x)==0 && sign(b.y - c.y)==0)
			{
				return -1;
			}
			if (sign(b.x -d.x)==0 && sign(b.y - d.y)==0)
			{
				return -1;
			}
			return 0;
		}
    }
	//如果不相交
    return -3;
}

 //求ab是否与cd相交，交点为p。1规范相交，0非共线的非规范相交且交点不是a -1共线的非规范相交 -2不相交 -3交点为a
template<typename T>
inline int Math_Is2SegInter2(T a,T b,T c,T d ,T &p) 
{
    double s1,s2,s3,s4;
    int d1,d2,d3,d4;
    d1=sign(s1=xmult(a,b,c));
    d2=sign(s2=xmult(a,b,d));
    d3=sign(s3=xmult(c,d,a));
    d4=sign(s4=xmult(c,d,b));
	
	//如果规范相交则求交点
    if ((d1^d2)==-2 && (d3^d4)==-2)
    {
        p.x=(c.x*s2-d.x*s1)/(s2-s1);
        p.y=(c.y*s2-d.y*s1)/(s2-s1);
        return 1;
    }
	
	//如果不规范相交 且无重合
    if (d1==0 && Math_PonSeg(c,a,b))
    {
        p=c;
		if (Math_PonSeg(a,c,d) || Math_PonSeg(b,c,d))
		{
			return -1;
		}
		else
		{
			return 0;
		}
    }
    if (d2==0 && Math_PonSeg(d,a,b))
    {
        p=d;
		if (Math_PonSeg(a,c,d) || Math_PonSeg(b,c,d))
		{
			return -1;
		}
		else
		{
			return 0;
		}
    }
    if (d3==0 && Math_PonSeg(a,c,d))
    {
        p=a;
        if (Math_PonSeg(c,a,b) || Math_PonSeg(d,a,b))
		{
			return -1;
		}
		else
		{
			return -3;
		}
    }
    if (d4==0 && Math_PonSeg(b,c,d))
    {
        p=b;
        if (Math_PonSeg(c,a,b) || Math_PonSeg(d,a,b))
		{
			return -1;
		}
		else
		{
			return 0;
		}
    }
	//如果不相交
    return -2;
}

//求直线交点，必须存在交点，或者预判断【解析几何方法】
template<typename T>
T Math_2SegInter(line<T> u,line<T> v) { 
    double a1=u.b.y-u.a.y,b1=u.a.x-u.b.x;
    double c1=u.b.y*(-b1)-u.b.x*a1;
    double a2=v.b.y-v.a.y,b2=v.a.x-v.b.x;
    double c2=v.b.y*(-b2)-v.b.x*a2;
    double D=xmult(a1,b1,a2,b2);
    return T(xmult(b1,c1,b2,c2)/D,xmult(c1,a1,c2,a2)/D);
}
//求线段交点，必须存在交点，或者预判断【平面几何方法】
template<typename T>
T Math_2SegInter2(line<T> u,line<T> v) { 
    T ret=u.a;
    double t=xmult(u.a-v.a,v.b-v.a)/xmult(u.b-u.a,v.b-v.a);
    t=fabs(t);
    ret.x+=(u.b.x-u.a.x)*t;
    ret.y+=(u.b.y-u.a.y)*t;
    return ret;
}

//线段之间最短距离
template<typename T>
inline double Math_Seg2SegDist(line<T> p,line<T> q) {
    return min(min(Math_P2SegDist(p.a,q,T()),Math_P2SegDist(p.b,q,T())),min(Math_P2SegDist(q.a,p,T()),Math_P2SegDist(q.b,p,T())));
}

//判点在线段上
template<typename T>
inline bool Math_IsPonSeg(T a,T p1,T p2) {
    if(fabs(xmult(a,p1,p2))<=eps&&(a.x-p1.x)*(a.x-p2.x)<eps&&(a.y-p1.y)*(a.y-p2.y)<eps)
        return true;
    return false;
}

//判点与线段所在直线方位关系 0在直线上 -1在直线左 2在直线右
template<typename T>
inline int Math_P2SegRelation(T a,T p1,T p2) 
{
	return sign(xmult(a,p1,p2));
}

// 判断点到直线垂足与线段的关系
template<typename T>
double Math_P2SegRelation2(T p,line<T> l) 
{ 
	line<T> tl; 
	tl.a=l.a; 
	tl.b=p; 
	return dmult(l.a,tl.b,l.b)/(Math_Distance(l.a,l.b)*Math_Distance(l.a,l.b)); 
} 
/*本函数是根据下面的公式写的，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 
*/ 

//三角形重心
template<typename T>
T barycenter(T a,T b,T c){
	T res((a.x+b.x+c.x)/3.0,(a.y+b.y+c.y)/3.0);
    return res;
}
//多边形重心
template<typename T>
T Math_barycenter(T p[],int n) {
    T ret,t;
    double t1=0,t2;
    int i;
    ret.x=ret.y=0;
    for (i=1; i<n-1; i++)
        if (fabs(t2=xmult(p[i+1],p[0],p[i]))>eps) {
            t=barycenter(p[0],p[i],p[i+1]);
            ret.x+=t.x*t2;
            ret.y+=t.y*t2;
            t1+=t2;
        }
		if (fabs(t1)>eps)
			ret.x/=t1,ret.y/=t1;
		return ret;
}


//判点在线段端点左方
template<typename T>
int p_on_segvex(T s,T p) {
    return fabs(p.y-s.y)<eps&&(p.x<=s.x+eps);
}
//判线段相交 <=:不规范相交
template<typename T>
int seg_inter(line<T> s,line<T> p) {
    double minx1=min(s.a.x,s.b.x),maxx1=max(s.a.x,s.b.x);
    double minx2=min(p.a.x,p.b.x),maxx2=max(p.a.x,p.b.x);
    double miny1=min(s.a.y,s.b.y),maxy1=max(s.a.y,s.b.y);
    double miny2=min(p.a.y,p.b.y),maxy2=max(p.a.y,p.b.y);
    if((minx1>maxx2+eps)||(minx2>maxx1+eps)||
		(miny1>maxy2+eps)||(miny2>maxy1+eps))
        return 0;
    else
        return sign(xmult(s.a,s.b,p.a)*xmult(s.a,s.b,p.b))<=0&&
		sign(xmult(p.a,p.b,s.a)*xmult(p.a,p.b,s.b))<=0;
}
//判点在多边形内部
template<typename T>
int Math_PinPolygon(T a,T p[],int n) {
    int count = 0;
	T tmp;
    line<T> s(tmp,tmp);
	line<T> ps(tmp,tmp);
    ps.a = a,ps.b = a;
    ps.b.x = inf;
    for(int i = 0; i < n; i++) {
        s.a = p[i];
        if(i + 1 < n)s.b = p[i+1];
        else s.b = p[0];
        if (s.a.y > s.b.y)swap(s.a,s.b);
        if (Math_IsPonSeg(a,s.a,s.b))return 2;
        if ((fabs(s.a.y-s.b.y)>eps)) {
            if (p_on_segvex(s.b,a)) count++;
            else if (seg_inter(ps,s))count++;
        }
    }
    if (count%2)return 1;
    return 0;
}


// 比较两个向量pt1和pt2分别与x轴向量(1, 0)的夹角  
template<typename T>
bool CompareVector(const T &pt1,const T &pt2) {  
    //求向量的模  
    float m1 = sqrt((float)(pt1.x * pt1.x + pt1.y * pt1.y));  
    float m2 = sqrt((float)(pt2.x * pt2.x + pt2.y * pt2.y));  
    //两个向量分别与(1, 0)求内积  
    float v1 = pt1.x / m1, v2 = pt2.x / m2;  
    return ( v1 > v2 || (v1 == v2 && m1 < m2) );  
} 

//获取点列凸包
//已做封闭处理
template<typename T>
void Math_GetConvexHull(std::vector<T> &vecSrc) {  
    //点集中至少应有3个点，才能构成多边形  
    if (vecSrc.size() < 3) { return; }  
    //查找基点  
    T ptBase = vecSrc.front(); //将第1个点预设为最小点  
    for (std::vector<T>::iterator iter = vecSrc.begin() + 1; iter != vecSrc.end(); ++iter) {  
        //如果当前点的y值小于最小点，或y值相等，x值较小  
        if ( iter->y < ptBase.y || (iter->y == ptBase.y && iter->x > ptBase.x) ) {  
            //将当前点作为最小点  
            ptBase = *iter;  
        }  
    }  
    //计算出各点与基点构成的向量  
    for (iter = vecSrc.begin(); iter != vecSrc.end();) {  
        //排除与基点相同的点，避免后面的排序计算中出现除0错误  
        if (*iter == ptBase) {  
            iter = vecSrc.erase(iter);  
        }  
        else {  
            //方向由基点到目标点  
            iter->x -= ptBase.x, iter->y -= ptBase.y;  
            ++iter;  
        }  
    }  
	CompareVector(vecSrc.at(0),vecSrc.at(0));
    //按各向量与横坐标之间的夹角排序  
    sort(vecSrc.begin(), vecSrc.end(), &CompareVector<T>);  
    //删除相同的向量  
    vecSrc.erase(unique(vecSrc.begin(), vecSrc.end()), vecSrc.end());  
    //计算得到首尾依次相联的向量  
    for (std::vector<T>::reverse_iterator riter = vecSrc.rbegin();  
	riter != vecSrc.rend() - 1; ++riter) {  
		std::vector<T>::reverse_iterator riNext = riter + 1;  
		//向量三角形计算公式  
		riter->x -= riNext->x, riter->y -= riNext->y;  
    }  
    //依次删除不在凸包上的向量  
    for (iter = vecSrc.begin() + 1; iter != vecSrc.end(); ++iter) {  
        //回溯删除旋转方向相反的向量，使用外积判断旋转方向  
        for (std::vector<T>::iterator iLast = iter - 1; iLast != vecSrc.begin();) {  
            int v1 = iter->x * iLast->y, v2 = iter->y * iLast->x;  
            //如果叉积小于0，则无没有逆向旋转  
            //如果叉积等于0，还需判断方向是否相逆  
            if (v1 < v2 || (v1 == v2 && iter->x * iLast->x > 0 &&  
                iter->y * iLast->y > 0)) { break; }  
            //删除前一个向量后，需更新当前向量，与前面的向量首尾相连  
            //向量三角形计算公式  
            iter->x += iLast->x, iter->y += iLast->y;  
            iLast = (iter = vecSrc.erase(iLast)) - 1;  
        }  
    }  
    //将所有首尾相连的向量依次累加，换算成坐标  
    vecSrc.front().x += ptBase.x, vecSrc.front().y += ptBase.y;  
    for (iter = vecSrc.begin() + 1; iter != vecSrc.end(); ++iter) {  
        iter->x += (iter - 1)->x, iter->y += (iter - 1)->y;  
    }  
    //添加基点，全部的凸包计算完成  
    vecSrc.push_back(ptBase);  
	vecSrc.push_back(vecSrc.at(0));
}  

//贝塞尔曲线离散
template<typename T>
void Math_BezierCurve(std::vector<T> &p,double x1, double y1, double x2,double y2,double x3,double y3,double x4,double y4)
{  
	T pt1(x1,y1);
	T pt2(x2,y2);
	T pt3(x3,y3);
	T pt4(x4,y4);
	double l = Math_Distance(pt1,pt2) + Math_Distance(pt2,pt3) + Math_Distance(pt3,pt4);
	double r = l / (2*MATH_PI)/2;
	int step = ceil(2*MATH_PI/(2*acos((r-0.01)/r)));

	
	T tmp0(x1,y1);
	p.push_back(tmp0); 
	double dx1 = x2 - x1;  
	double dy1 = y2 - y1;  
	double dx2 = x3 - x2;  
	double dy2 = y3 - y2;  
	double dx3 = x4 - x3;  
	double dy3 = y4 - y3;  
	
	double subdiv_step  = 1.0 / (step + 1);  
	double subdiv_step2 = subdiv_step*subdiv_step;  
	double subdiv_step3 = subdiv_step*subdiv_step*subdiv_step;  
	
	double pre1 = 3.0 * subdiv_step;  
	double pre2 = 3.0 * subdiv_step2;  
	double pre4 = 6.0 * subdiv_step2;  
	double pre5 = 6.0 * subdiv_step3;  
	
	double tmp1x = x1 - x2 * 2.0 + x3;  
	double tmp1y = y1 - y2 * 2.0 + y3;  
	
	double tmp2x = (x2 - x3)*3.0 - x1 + x4;  
	double tmp2y = (y2 - y3)*3.0 - y1 + y4;  
	
	double fx = x1;  
	double fy = y1;  
	
	double dfx = (x2 - x1)*pre1 + tmp1x*pre2 + tmp2x*subdiv_step3;  
	double dfy = (y2 - y1)*pre1 + tmp1y*pre2 + tmp2y*subdiv_step3;  
	
	double ddfx = tmp1x*pre4 + tmp2x*pre5;  
	double ddfy = tmp1y*pre4 + tmp2y*pre5;  
	
	double dddfx = tmp2x*pre5;  
	double dddfy = tmp2y*pre5;  
	
	
	while(step--)  
	{  
		fx   += dfx;  
		fy   += dfy;  
		dfx  += ddfx;  
		dfy  += ddfy;  
		ddfx += dddfx;  
		ddfy += dddfy;  
		T tmp1(fx,fy);
		p.push_back(tmp1);  
	}  
	T tmp2(x4,y4);
	p.push_back(tmp2); 
} 

//已知圆上三点求圆心半径(局限性：斜率不存在时不可用该算法)
/*
template<typename T>
void Math_findCircle( T pt1, T pt2, T pt3, T &center, double &r)
{
	T midPt1, midPt2;
	midPt1.x = (pt2.x + pt1.x)/2;
	midPt1.y = (pt2.y + pt1.y)/2;
	
	midPt2.x = ( pt3.x + pt1.x)/2;
	midPt2.y = ( pt3.y + pt1.y)/2;
	
	float k1 = -(pt2.x - pt1.x)/(pt2.y - pt1.y);
	float k2 = -(pt3.x - pt1.x)/(pt3.y - pt1.y);
	

	center.x = (midPt2.y - midPt1.y- k2* midPt2.x + k1*midPt1.x)/(k1 - k2);
	center.y = midPt1.y + k1*( midPt2.y - midPt1.y - k2*midPt2.x + k2*midPt1.x)/(k1-k2);
	r = sqrtf((center.x - pt1.x)*(center.x - pt1.x) + (center.y - pt1.y)*(center.y - pt1.y));
}*/

//已知圆上三点求圆心半径
template<typename T>
void Math_GetCircular(T P1,T P2,T P3,T &center, double &r)
{
	float a = 2 * (P2.x - P1.x);
	float b = 2 * (P2.y - P1.y);
	float c = P2.x * P2.x + P2.y * P2.y - P1.x * P1.x - P1.y * P1.y;
	float d = 2 * (P3.x - P2.x);
	float e = 2 * (P3.y - P2.y);
	float f = P3.x * P3.x + P3.y * P3.y - P2.x * P2.x - P2.y * P2.y;
	center.x = (b * f - e * c) / (b * d - e * a);
	center. y = (d * c - a * f) / (b * d - e * a);
	r = sqrtf( (double)((center.x - P1.x) * (center.x - P1.x) + (center.y - P1.y) * (center.y - P1.y)));
}

//已知圆弧上三点求圆弧时针方向
template<typename T>
void Math_ArcClock(T P1,T P2,T P3, bool &bccw)
{
	T p12(P2.x - P1.x, P2.y - P1.y);
	T p23(P3.x - P2.x, P3.y - P2.y);

	if (sign(xmult(p12,p23)) > 0)
	{
		bccw = true;
	}
	else
	{
		bccw = false;
	}
}



template<typename T>
void Math_BezierCurve1(std::vector<T> &p,double x1, double y1, double x2,double y2,double x3,double y3,double x4,double y4)
{
	int n = 4;//P[0]-P[n-1]参与绘制Bezier曲线
	T PO;
	double t;
	T P[7];
	for (t = 0; t < 1; t += 0.001)
	{
		//每一次运算都要重新赋一次初始值
		P[0].Set(x1, y1);
		P[1].Set(x2, y2);
		P[2].Set(x3, y3);
		P[3].Set(x4, y4);
		//参照n的值，一直赋初始值到P[n-1]
		
		int x = n;
		while (1)
		{
			if (x == 1)
				break;
			for (int i = 0; i < x - 1; i++)
			{
				PO.x = ((1 - t) * P[i].x + t * P[i + 1].x);
				PO.y = ((1 - t) * P[i].y + t * P[i + 1].y);
				P[i] = PO;
			}
			x--;
		}
		T tmp0(P[0].x,P[0].y);
		p.push_back(tmp0);
	}
}

//贝塞尔曲线离散
template<typename T>
void Math_BezierCurve(std::vector<T> &p,double x1, double y1, double x2,double y2,double x3,double y3,double x4,double y4,double accuracy)
{  
	T pt1(x1,y1);
	T pt2(x2,y2);
	T pt3(x3,y3);
	T pt4(x4,y4);
	double l = Math_Distance(pt1,pt2) + Math_Distance(pt2,pt3) + Math_Distance(pt3,pt4);
	double r = l / (2*MATH_PI)/2;
	int step = ceil(2*MATH_PI/(2*acos((r-accuracy)/r)));
	
	
	T tmp0(x1,y1);
	p.push_back(tmp0); 
	double dx1 = x2 - x1;  
	double dy1 = y2 - y1;  
	double dx2 = x3 - x2;  
	double dy2 = y3 - y2;  
	double dx3 = x4 - x3;  
	double dy3 = y4 - y3;  
	
	double subdiv_step  = 1.0 / (step + 1);  
	double subdiv_step2 = subdiv_step*subdiv_step;  
	double subdiv_step3 = subdiv_step*subdiv_step*subdiv_step;  
	
	double pre1 = 3.0 * subdiv_step;  
	double pre2 = 3.0 * subdiv_step2;  
	double pre4 = 6.0 * subdiv_step2;  
	double pre5 = 6.0 * subdiv_step3;  
	
	double tmp1x = x1 - x2 * 2.0 + x3;  
	double tmp1y = y1 - y2 * 2.0 + y3;  
	
	double tmp2x = (x2 - x3)*3.0 - x1 + x4;  
	double tmp2y = (y2 - y3)*3.0 - y1 + y4;  
	
	double fx = x1;  
	double fy = y1;  
	
	double dfx = (x2 - x1)*pre1 + tmp1x*pre2 + tmp2x*subdiv_step3;  
	double dfy = (y2 - y1)*pre1 + tmp1y*pre2 + tmp2y*subdiv_step3;  
	
	double ddfx = tmp1x*pre4 + tmp2x*pre5;  
	double ddfy = tmp1y*pre4 + tmp2y*pre5;  
	
	double dddfx = tmp2x*pre5;  
	double dddfy = tmp2y*pre5;  
	
	
	while(step--)  
	{  
		fx   += dfx;  
		fy   += dfy;  
		dfx  += ddfx;  
		dfy  += ddfy;  
		ddfx += dddfx;  
		ddfy += dddfy;  
		T tmp1(fx,fy);
		p.push_back(tmp1);  
	}  
	T tmp2(x4,y4);
	p.push_back(tmp2); 
} 

//椭圆离散 XY圆心坐标 r1长轴，r2短轴，alpha偏转角度，StartAngle、EndAngle起始终止角度，Accuracy精度
template<typename T>
void Math_DiscretizeEllipse(std::vector<T> &vertices,double x, double y, double r1,double r2, 
							double alpha, double StartAngle, double EndAngle, double Accuracy)
{
	int seg = max(max((int)floor(sqrt((r1+r2)/Accuracy)), 1.0/Accuracy),100);
    float angle_shift = (fabs(EndAngle - StartAngle)) / seg, phi = 0;
	
	int Start_Shift = (int)StartAngle / angle_shift;
	if(StartAngle<EndAngle)
	{
		phi = Start_Shift * angle_shift;
		for (int i = 0; i < seg; ++i) {
			phi += angle_shift;
			double x1 = r1 * cos(phi);
			double y1 = r2 * sin(phi);
			double x2 = x1 * cos(alpha) - y1 * sin(alpha);
			double y2 = x1 * sin(alpha) + y1 * cos(alpha);
			x2 = x2 + x;
			y2 = y2 + y;
			T tmp1(x2,y2);
			vertices.push_back(tmp1);
		}
	}
	else
	{
		phi = Start_Shift * angle_shift;
		for (int i = 0; i < seg; ++i) {
			phi -= angle_shift;
			double x1 = r1 * cos(phi);
			double y1 = r2 * sin(phi);
			double x2 = x1 * cos(alpha) - y1 * sin(alpha);
			double y2 = x1 * sin(alpha) + y1 * cos(alpha);
			x2 = x2 + x;
			y2 = y2 + y;
			T tmp1(x2,y2);
			vertices.push_back(tmp1);
		}
	}
	
}