gnode.h

是一个G代码转换的源程序

void GNode::LLComp(const float &Xq1,const float &Yq1,const float &Xz1,const float &Yz1,
			       const float &Xq2,const float &Yq2,const float &Xz2,const float &Yz2,
		           float rc,const int &RadialComp,vector<CompCoord> &result)
{
	double Angle1, Angle2,Angle3;
	float lrxq,lryq,lrxz,lryz;
	float ACx,ACy,AC1x,AC1y,AC2x,AC2y;
	double cosa,sina;
	CompCoord Value;
	result.clear();
	Angle1=Angle(Xq1,Yq1,Xz1,Yz1);
	Angle2=Angle(Xq2,Yq2,Xz2,Yz2);
	Angle3=Angle2-Angle1;
	Angle3=Angle3*pi/180;
	sina=sin(Angle3);
	cosa=cos(Angle3);
	if ((sina>-Epsilon || (sina<0 && cosa>-Epsilon)) && RadialComp==1)
		// G41缩短型和伸长型
	{
		LineComp(Xq1,Yq1,Xz1,Yz1,rc,RadialComp,lrxq,lryq,lrxz,lryz);
		LL1Radius(Angle1,Angle2,rc,RadialComp,ACx,ACy);
		Value.Typ=1;
		Value.xq=lrxq;
		Value.yq=lryq;
		Value.xz=Xz1+ACx;
		Value.yz=Yz1+ACy;
		result.push_back(Value);
		LineComp(Xq2,Yq2,Xz2,Yz2,rc,RadialComp,lrxq,lryq,lrxz,lryz);
		Value.Typ=1;
		Value.xq=Xq2+ACx;
		Value.yq=Yq2+ACy;
		Value.xz=lrxz;
		Value.yz=lryz;
		result.push_back(Value);
	}
	else if ((sina<0 && cosa<0) && RadialComp==1)
		// G41插入型	
	{
		LineComp(Xq1,Yq1,Xz1,Yz1,rc,RadialComp,lrxq,lryq,lrxz,lryz);
		LL2Radius(Angle1,Angle2,rc,RadialComp,AC1x,AC1y,AC2x,AC2y);
		Value.Typ=1;
		Value.xq=lrxq;
		Value.yq=lryq;
		Value.xz=Xz1+AC1x;
		Value.yz=Yz1+AC1y;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+AC1x;
		Value.yq=Yz1+AC1y;
		Value.xz=Xq2+AC2x;
		Value.yz=Yq2+AC2y;
		result.push_back(Value);
		LineComp(Xq2,Yq2,Xz2,Yz2,rc,RadialComp,lrxq,lryq,lrxz,lryz);
		Value.Typ=1;
		Value.xq=Xq2+AC2x;
		Value.yq=Yq2+AC2y;
		Value.xz=lrxz;
		Value.yz=lryz;
		result.push_back(Value);
	}
	else if ((sina<0 || (sina>-Epsilon && cosa>-Epsilon)) && RadialComp==2)
		// G42缩短型和伸长型
	{
		LineComp(Xq1,Yq1,Xz1,Yz1,rc,RadialComp,lrxq,lryq,lrxz,lryz);
		LL1Radius(Angle1,Angle2,rc,RadialComp,ACx,ACy);
		Value.Typ=1;
		Value.xq=lrxq;
		Value.yq=lryq;
		Value.xz=Xz1+ACx;
		Value.yz=Yz1+ACy;
		result.push_back(Value);
		LineComp(Xq2,Yq2,Xz2,Yz2,rc,RadialComp,lrxq,lryq,lrxz,lryz);
		Value.Typ=1;
		Value.xq=Xq2+ACx;
		Value.yq=Yq2+ACy;
		Value.xz=lrxz;
		Value.yz=lryz;
		result.push_back(Value);
	}
	else if ((sina>-Epsilon && cosa<0) && RadialComp==2)
		// G42插入型号
	{
		LineComp(Xq1,Yq1,Xz1,Yz1,rc,RadialComp,lrxq,lryq,lrxz,lryz);
		LL2Radius(Angle1,Angle2,rc,RadialComp,AC1x,AC1y,AC2x,AC2y);
		Value.Typ=1;
		Value.xq=lrxq;
		Value.yq=lryq;
		Value.xz=Xz1+AC1x;
		Value.yz=Yz1+AC1y;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+AC1x;
		Value.yq=Yz1+AC1y;
		Value.xz=Xq2+AC2x;
		Value.yz=Yq2+AC2y;
		result.push_back(Value);
		LineComp(Xq2,Yq2,Xz2,Yz2,rc,RadialComp,lrxq,lryq,lrxz,lryz);
		Value.Typ=1;
		Value.xq=Xq2+AC2x;
		Value.yq=Yq2+AC2y;
		Value.xz=lrxz;
		Value.yz=lryz;
		result.push_back(Value);
	}
}

void GNode::LineComp(const float &xq,const float &yq,const float &xz,const float &yz,
				     float rc,const int &RadialComp,
                     float &lrxq,float &lryq,float &lrxz,float &lryz)
{
	double angle;
	angle=Angle(xq,yq,xz,yz);
	angle=angle*pi/180;
	rc=fabs(rc);
	if (RadialComp==2)
		rc=-1*rc;
	lrxq=float(xq-rc*sin(angle));
	lryq=float(yq+rc*cos(angle));
	lrxz=float(xz-rc*sin(angle));
	lryz=float(yz+rc*cos(angle));
}

void GNode::LL1Radius(double angle1,double angle2,double rc,const int &RadialComp,
					  float &ACx,float &ACy)
{
	rc=fabs(rc);
	angle1=angle1*pi/180;
	angle2=angle2*pi/180;	
	if (RadialComp==1)
		rc=rc*(-1);
	ACx=float(rc*(sin(angle1)+sin(angle2))/(1+cos(angle2-angle1)));
	ACy=float(rc*(-cos(angle1)-cos(angle2))/(1+cos(angle2-angle1)));
}

void GNode::LL2Radius(double angle1,double angle2,double rc,const int &RadialComp,
					  float &AC1x,float &AC1y,float &AC2x,float &AC2y)
{
	rc=fabs(rc);
	angle1=angle1*pi/180;
	angle2=angle2*pi/180;	
	if (RadialComp==1)
	{
		AC1x=float(rc*(cos(angle1)-sin(angle1)));
		AC1y=float(rc*(cos(angle1)+sin(angle1)));
		AC2x=float(rc*(-sin(angle2)-cos(angle2)));
		AC2y=float(rc*(-sin(angle2)+cos(angle2)));
	}
	else if (RadialComp==2)
	{
		AC1x=float(rc*(sin(angle1)+cos(angle1)));
		AC1y=float(rc*(sin(angle1)-cos(angle1)));
		AC2x=float(rc*(sin(angle2)-cos(angle2)));
		AC2y=float(rc*(-sin(angle2)-cos(angle2)));
	}
}

void GNode::CCComp(const float &Xq1,const float &Yq1,const float &Xz1,const float &Yz1,
				   const float &Ox1,const float &Oy1,float r1,const int &Typ1,
				   const float &Xq2,const float &Yq2,const float &Xz2,const float &Yz2,
				   const float &Ox2,const float &Oy2,float r2,const int &Typ2,
				   float rc,const int &RadialComp,vector<CompCoord> &result)
{
	double Angle1,Angle2,Angle3;
	float lrxq,lryq,lrxz,lryz;
	float ABx,ABy,AC1x,AC1y,AC2x,AC2y,ADx,ADy;
	double cosa,sina;
	float x1,y1,x2,y2;
	float xj,yj;
	double Length1,Length2;
	float rc1,rc2;
	rc1=rc;
	rc2=rc;
	CompCoord Value;
	result.clear();
	Angle1=dqcy(Xz1,Yz1,Ox1,Oy1,Typ1);
	Angle2=dqcy(Xq2,Yq2,Ox2,Oy2,Typ2);
	Angle3=Angle2-Angle1;
	Angle3=Angle3*pi/180;
	sina=sin(Angle3);
	cosa=cos(Angle3);
	r1=fabs(r1);
	r2=fabs(r2);
	if (sina>-Epsilon && RadialComp==1)
		// G41缩短型
	{
		ARCRadius(Xq1,Yq1,Xz1,Yz1,Ox1,Oy1,r1,RadialComp,Typ1,rc1,lrxq,lryq,lrxz,lryz);
		Value.Typ=Typ1;
		Value.xq=lrxq;
		Value.yq=lryq;
		Value.xc=Ox1;
		Value.yc=Oy1;
		Value.r=r1+rc1;
		ARCRadius(Xq2,Yq2,Xz2,Yz2,Ox2,Oy2,r2,RadialComp,Typ2,rc2,lrxq,lryq,lrxz,lryz);
		JC1JC2(Ox1,Oy1,r1+rc1,Ox2,Oy2,r2+rc2,x1,y1,x2,y2);
		Length1=sqrt((x1-Xz1)*(x1-Xz1)+(y1-Yz1)*(y1-Yz1));
		Length2=sqrt((x2-Xz1)*(x2-Xz1)+(y2-Yz1)*(y2-Yz1));
		if (Length1<Length2)
		{
			xj=x1;
			yj=y1;
		}
		else
		{
			xj=x2;
			yj=y2;
		}
		Value.xz=xj;
		Value.yz=yj;
		result.push_back(Value);
		Value.Typ=Typ2;
		Value.xq=xj;
		Value.yq=yj;
		Value.xc=Ox2;
		Value.yc=Oy2;
		Value.r=r2+rc2;
		Value.xz=lrxz;
		Value.yz=lryz;
		result.push_back(Value);
	}
	else if ((sina<0 && cosa>-Epsilon) &&  RadialComp==1)
		// G41伸长型
	{
		ARCRadius(Xq1,Yq1,Xz1,Yz1,Ox1,Oy1,r1,RadialComp,Typ1,rc,lrxq,lryq,lrxz,lryz);
		Value.Typ=Typ1;
		Value.xq=lrxq;
		Value.yq=lryq;
		Value.xc=Ox1;
		Value.yc=Oy1;
		Value.r=r1+rc;
		CC1Radius(Angle1,Angle2,rc,RadialComp,ABx,ABy,AC1x,AC1y,AC2x,AC2y);
		Value.xz=Xz1+ABx;
		Value.yz=Yz1+ABy;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+ABx;
		Value.yq=Yz1+ABy;
		Value.xz=Xz1+AC1x;
		Value.yz=Yz1+AC1y;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+AC1x;
		Value.yq=Yz1+AC1y;
		Value.xz=Xz1+AC2x;
		Value.yz=Yz1+AC2y;
		result.push_back(Value);
		ARCRadius(Xq2,Yq2,Xz2,Yz2,Ox2,Oy2,r2,RadialComp,Typ2,rc,lrxq,lryq,lrxz,lryz);
		Value.Typ=Typ2;
		Value.xq=Xz1+AC2x;
		Value.yq=Yz1+AC2y;
		Value.xc=Ox2;
		Value.yc=Oy2;
		Value.r=r2+rc;
		Value.xz=lrxz;
		Value.yz=lryz;
		result.push_back(Value);
	}
	else if ((sina<0 && cosa<0) &&  RadialComp==1)
		// G41插入型
	{
		ARCRadius(Xq1,Yq1,Xz1,Yz1,Ox1,Oy1,r1,RadialComp,Typ1,rc,lrxq,lryq,lrxz,lryz);
		Value.Typ=Typ1;
		Value.xq=lrxq;
		Value.yq=lryq;
		Value.xc=Ox1;
		Value.yc=Oy1;
		Value.r=r1+rc;
		CC2Radius(Angle1,Angle2,rc,RadialComp,ABx,ABy,AC1x,AC1y,AC2x,AC2y,ADx,ADy);
		Value.xz=Xz1+ABx;
		Value.yz=Yz1+ABy;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+ABx;
		Value.yq=Yz1+ABy;
		Value.xz=Xz1+AC1x;
		Value.yz=Yz1+AC1y;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+AC1x;
		Value.yq=Yz1+AC1y;
		Value.xz=Xz1+AC2x;
		Value.yz=Yz1+AC2y;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+AC2x;
		Value.yq=Yz1+AC2y;
		Value.xz=Xz1+ADx;
		Value.yz=Yz1+ADy;
		result.push_back(Value);
		ARCRadius(Xq2,Yq2,Xz2,Yz2,Ox2,Oy2,r2,RadialComp,Typ2,rc,lrxq,lryq,lrxz,lryz);
		Value.Typ=Typ2;
		Value.xq=Xz1+ADx;
		Value.yq=Yz1+ADy;
		Value.xc=Ox2;
		Value.yc=Oy2;
		Value.r=r2+rc;
		Value.xz=lrxz;
		Value.yz=lryz;
		result.push_back(Value);
	}
	else if (sina<0 && RadialComp==2)
		// G42缩短型
	{
		ARCRadius(Xq1,Yq1,Xz1,Yz1,Ox1,Oy1,r1,RadialComp,Typ1,rc1,lrxq,lryq,lrxz,lryz);
		Value.Typ=Typ1;
		Value.xq=lrxq;
		Value.yq=lryq;
		Value.xc=Ox1;
		Value.yc=Oy1;
		Value.r=r1+rc1;
		ARCRadius(Xq2,Yq2,Xz2,Yz2,Ox2,Oy2,r2,RadialComp,Typ2,rc2,lrxq,lryq,lrxz,lryz);
		JC1JC2(Ox1,Oy1,r1+rc1,Ox2,Oy2,r2+rc2,x1,y1,x2,y2);
		Length1=sqrt((x1-Xz1)*(x1-Xz1)+(y1-Yz1)*(y1-Yz1));
		Length2=sqrt((x2-Xz1)*(x2-Xz1)+(y2-Yz1)*(y2-Yz1));
		if (Length1<Length2)
		{
			xj=x1;
			yj=y1;
		}
		else
		{
			xj=x2;
			yj=y2;
		}
		Value.xz=xj;
		Value.yz=yj;
		result.push_back(Value);
		Value.Typ=Typ2;
		Value.xq=xj;
		Value.yq=yj;
		Value.xc=Ox2;
		Value.yc=Oy2;
		Value.r=r2+rc2;
		Value.xz=lrxz;
		Value.yz=lryz;
		result.push_back(Value);
	}
	else if ((sina>-Epsilon && cosa>-Epsilon) && RadialComp==2)
		// G42伸长型
	{
		ARCRadius(Xq1,Yq1,Xz1,Yz1,Ox1,Oy1,r1,RadialComp,Typ1,rc,lrxq,lryq,lrxz,lryz);
		Value.Typ=Typ1;
		Value.xq=lrxq;
		Value.yq=lryq;
		Value.xc=Ox1;
		Value.yc=Oy1;
		Value.r=r1+rc;
		CC1Radius(Angle1,Angle2,rc,RadialComp,ABx,ABy,AC1x,AC1y,AC2x,AC2y);
		Value.xz=Xz1+ABx;
		Value.yz=Yz1+ABy;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+ABx;
		Value.yq=Yz1+ABy;
		Value.xz=Xz1+AC1x;
		Value.yz=Yz1+AC1y;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+AC1x;
		Value.yq=Yz1+AC1y;
		Value.xz=Xz1+AC2x;
		Value.yz=Yz1+AC2y;
		result.push_back(Value);
		ARCRadius(Xq2,Yq2,Xz2,Yz2,Ox2,Oy2,r2,RadialComp,Typ2,rc,lrxq,lryq,lrxz,lryz);
		Value.Typ=Typ2;
		Value.xq=Xz1+AC2x;
		Value.yq=Yz1+AC2y;
		Value.xc=Ox2;
		Value.yc=Oy2;
		Value.r=r2+rc;
		Value.xz=lrxz;
		Value.yz=lryz;
		result.push_back(Value);
	}
	else if ((sina>-Epsilon && cosa<0) && RadialComp==2)
		// G42插入型
	{
		ARCRadius(Xq1,Yq1,Xz1,Yz1,Ox1,Oy1,r1,RadialComp,Typ1,rc,lrxq,lryq,lrxz,lryz);
		Value.Typ=Typ1;
		Value.xq=lrxq;
		Value.yq=lryq;
		Value.xc=Ox1;
		Value.yc=Oy1;
		Value.r=r1+rc;
		CC2Radius(Angle1,Angle2,rc,RadialComp,ABx,ABy,AC1x,AC1y,AC2x,AC2y,ADx,ADy);
		Value.xz=Xz1+ABx;
		Value.yz=Yz1+ABy;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+ABx;
		Value.yq=Yz1+ABy;
		Value.xz=Xz1+AC1x;
		Value.yz=Yz1+AC1y;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+AC1x;
		Value.yq=Yz1+AC1y;
		Value.xz=Xz1+AC2x;
		Value.yz=Yz1+AC2y;
		result.push_back(Value);
		Value.Typ=1;
		Value.xq=Xz1+AC2x;
		Value.yq=Yz1+AC2y;
		Value.xz=Xz1+ADx;
		Value.yz=Yz1+ADy;
		result.push_back(Value);
		ARCRadius(Xq2,Yq2,Xz2,Yz2,Ox2,Oy2,r2,RadialComp,Typ2,rc,lrxq,lryq,lrxz,lryz);
		Value.Typ=Typ2;
		Value.xq=Xz1+ADx;
		Value.yq=Yz1+ADy;
		Value.xc=Ox2;
		Value.yc=Oy2;
		Value.r=r2+rc;
		Value.xz=lrxz;
		Value.yz=lryz;
		result.push_back(Value);
	}
}

void GNode::ARCRadius(const float &Xq,const float &Yq,const float &Xz,const float &Yz,
					  const float &Ox,const float &Oy,float r,const int &RadialComp,const int &Typ,
					  float &rc,float &lrxq,float &lryq,float &lrxz,float &lryz)
{
	r=fabs(r);
	rc=fabs(rc);
	if (Typ==2)		// 瞬时值
	{
		if (RadialComp==2)		// G42
			rc=-1*rc;
	}
	else
	{
		if (RadialComp==1)		// G41
			rc=-1*rc;
	}
	lrxq=Xq+rc*(Xq-Ox)/r;
	lryq=Yq+rc*(Yq-Oy)/r;
	lrxz=Xz+rc*(Xz-Ox)/r;
	lryz=Yz+rc*(Yz-Oy)/r;
}

void GNode::CC1Radius(double Angle1,double Angle2,float rc,const int &RadialComp,
					  float &ABx,float &ABy,float &AC1x,float &AC1y,float &AC2x,float &AC2y)
{
	Angle1=Angle1*pi/180;
	Angle2=Angle2*pi/180;
	rc=fabs(rc);
	if (RadialComp==1)
		rc=-1*rc;
	ABx=float(rc*sin(Angle1));
	ABy=float(-rc*cos(Angle1));
	AC1x=float(rc*(sin(Angle1)+sin(Angle2))/(1+cos(Angle2-Angle1)));
	AC1y=float(rc*(-cos(Angle1)-cos(Angle2))/(1+cos(Angle2-Angle1)));
	AC2x=float(rc*sin(Angle2));
	AC2y=float(-rc*cos(Angle2));
}

void GNode::CC2Radius(double Angle1,double Angle2,float rc,const int &RadialComp,
					  float &ABx,float &ABy,float &AC1x,float &AC1y,float &AC2x,float &AC2y,float &ADx,float &ADy)
{
	Angle1=Angle1*pi/180;
	Angle2=Angle2*pi/180;
	rc=fabs(rc);	
	if (RadialComp==1)
	{
		ABx=float(-rc*sin(Angle1));
		ABy=float(rc*cos(Angle1));
		AC1x=float(rc*(cos(Angle1)-sin(Angle1)));
		AC1y=float(rc*(cos(Angle1)+sin(Angle1)));
		AC2x=float(rc*(-sin(Angle2)-cos(Angle2)));
		AC2y=float(rc*(-sin(Angle2)+cos(Angle2)));
		ADx=float(-rc*sin(Angle2));
		ADy=float(rc*cos(Angle2));
	}
	else if (RadialComp==2)
	{
		ABx=float(rc*sin(Angle1));
		ABy=float(-rc*cos(Angle1));
		AC1x=float(rc*(sin(Angle1)+cos(Angle1)));
		AC1y=float(rc*(sin(Angle1)-cos(Angle1)));
		AC2x=float(rc*(sin(Angle2)-cos(Angle2)));
		AC2y=float(rc*(-sin(Angle2)-cos(Angle2)));
		ADx=float(rc*sin(Angle2));
		ADy=float(-rc*cos(Angle2));
	}
}

void GNode::LCComp(const float &Xq1,const float &Yq1,const float &Xz1,const float &Yz1,
                   const float &Xq2,const float &Yq2,const float &Xz2,const float &Yz2,
				   const float &Ox2,const float &Oy2,float r2,const int &Typ2,
				   float rc,const int &RadialComp,vector<CompCoord> &result)
{
	double Angle1,Angle2,Angle3;
	float xq,yq,xz,yz,lrxq,lryq,lrxz,lryz;
	float AC1x,AC1y,AC2x,AC2y,ADx,ADy;
	double cosa,sina;
	float x1,y1,x2,y2;
	float xj,yj;
	double Length1,Length2;
	CompCoord Value;
	r2=fabs(r2);
	result.clear();
	Angle1=Angle(Xq1,Yq1,Xz1,Yz1);
	Angle2=dqcy(Xq2,Yq2,Ox2,Oy2,Typ2);
	Angle3=Angle2-Angle1;
	Angle3=Angle3*pi/180;
	sina=sin(Angle3);
	cosa=cos(Angle3);
	if (sina>-Epsilon && RadialComp==1)
		// G41缩短型
	{
		LineComp(Xq1,Yq1,Xz1,Yz1,rc,RadialComp,lrxq,lryq,lrxz,lryz);
		xq=lrxq;
		yq=lryq;
		xz=lrxz;
		yz=lryz;
		ARCRadius(Xq2,Yq2,Xz2,Yz2,Ox2,Oy2,r2,RadialComp,Typ2,rc,lrxq,lryq,lrxz,lryz);
		LineJC(xq,yq,xz,yz,Ox2,Oy2,r2+rc,x1,y1,x2,y2);
		Length1=sqrt((x1-Xz1)*(x1-Xz1)+(y1-Yz1)*(y1-Yz1));
		Length2=sqrt((x2-Xz1)*(x2-Xz1)+(y2-Yz1)*(y2-Yz1));
		if (Length1<Length2)
		{
			xj=x1;
			yj=y1;
		}
		else
		{
			xj=x2;