gnode.h

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

		a=(yz-yq)/(xz-xq);
		b=(yq*xz-yz*xq)/(xz-xq);
		c=b-Oy;
		D=1+a*a;
		E=2*(a*c-Ox);
		F=Ox*Ox+c*c-r*r;
		if (E*E-4*D*F>0 || fabs(E*E-4*D*F)<Epsilon)
		{
			x1=float((-E+sqrt(fabs(E*E-4*D*F)))/(2*D));
			x2=float((-E-sqrt(fabs(E*E-4*D*F)))/(2*D));
		}
		y1=float(a*x1+b);
		y2=float(a*x2+b);
	}
	else
	{
		if ((r*r-(xq-Ox)*(xq-Ox))>0 || fabs((r*r-(xq-Ox)*(xq-Ox)))<Epsilon)
		{
			x1=xq;
			x2=xq;
			y1=sqrt(fabs(r*r-(xq-Ox)*(xq-Ox)))+Oy;
			y2=-sqrt(fabs(r*r-(xq-Ox)*(xq-Ox)))+Oy;
		}
	}
}

double GNode::dqcy(const float &xj,const float &yj,const float &Ox,const float &Oy,const int &Typ)
{
	double angle;
	angle=Angle(Ox,Oy,xj,yj);
	if (Typ==2)
		angle=angle-90;
	if (Typ==3)
		angle=angle+90;
	if (angle>360)
		angle=angle-360;
	if (angle<0)
		angle=angle+360;
	return angle;
}

void GNode::CalCenter(const float &xq,const float &yq,const float &xz,const float &yz,
					  const float &r1,const int &Typ,float &xc,float &yc)
{
	// 问题处
	float a,b,c,xc1,xc2,yc1,yc2,r;
	float x,y;
	float tmp;
	xc=0;yc=0;
	x=xz-xq;	// 将圆弧起点移动到圆心
	y=yz-yq;
	r=fabs(r1);
	a=4*(x*x+y*y);
	b=-4*(x*y*y+x*x*x);
	c=(x*x+y*y)*(x*x+y*y)-4*r*r*y*y;
	tmp=b*b-4*a*c;
    if  (b*b-4*a*c>-0.01)
	{
        xc1=(-b+sqrt(fabs(b*b-4*a*c)))/(2*a);
        xc2=(-b-sqrt(fabs(b*b-4*a*c)))/(2*a);
	}
	else
		exit(1);
	if (x >= 0 && y > 0)
	{
		if (xc1*xc2<0) 
		{
			if (xc1>0)
			{
				if (r1<0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
			else
			{
				if (r1>0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
		}
		else
		{
			if (fabs(xc1) > fabs(xc2))
			{
				if (r1<0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
			else
			{
				if (r1>0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
		}
	}
	else if (x<0 && y>=0)
	{
		if (xc1*xc2<0)
		{
			if (xc1>=0)
			{
				if (r1<0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
			else
			{
				if (r1>0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
		}
		else
		{
			if (fabs(xc1) < fabs(xc2))
			{
				if (r1<0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
			else
			{
				if (r1>0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
		}
	}
	else if (x<=0 && y<0)
	{
		if (xc1*xc2<0)
		{
			if (xc1<0)
			{
				if (r1<0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
			else
			{
				if (r1>0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
		}
		else
		{
			if (fabs(xc1) > fabs(xc2))
			{
				if (r1<0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
			else
			{
				if (r1>0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
		}
	}
	else if (x>0 && y<=0)
	{
		if (xc1*xc2<0)
		{
			if (xc1<=0)
			{
				if (r1<0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
			else
			{
				if (r1>0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
		}
		else
		{
			if (fabs(xc1)<fabs(xc2))
			{
				if (r1<0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
			else
			{
				if (r1>0)
				{
					tmp=xc2;
					xc2=xc1;
					xc1=tmp;
				}
			}
		}
	}
	if (Typ==2)
		xc=xc1;
	else if (Typ==3)
		xc=xc2;
	yc1=sqrtf(r*r-xc*xc);
	yc2=-sqrtf(r*r-xc*xc);

	double tmp1=fabs((x-xc)*(x-xc)+(y-yc1)*(y-yc1)-r*r);
	double tmp2=fabs((x-xc)*(x-xc)+(y-yc2)*(y-yc2)-r*r);
	if (tmp1<0.01)
	{
		yc=yc1;
		xc=xc+xq;
		yc=yc+yq;
		double AngleQ=Angle(xc,yc,xq,yq);
		double AngleZ=Angle(xc,yc,xz,yz);
		double ZAngle;
		if (AngleZ>AngleQ)
			ZAngle=AngleZ-AngleQ;
		else
		{
			ZAngle=360+AngleZ-AngleQ;
		}
		if (Typ==2)
				ZAngle=360-ZAngle;
		if (ZAngle>180)
		{
			if (r1>0)
			{
				yc=yc2+yq;
			}
		}
		else
		{
			if (r1<0)
			{
				yc=yc2+yq;
			}
		}
	}
	else
	{
		yc=yc2;
		xc=xc+xq;
		yc=yc+yq;
		double AngleQ=Angle(xc,yc,xq,yq);
		double AngleZ=Angle(xc,yc,xz,yz);
		double ZAngle;
		if (AngleZ>AngleQ)
			ZAngle=AngleZ-AngleQ;
		else
			ZAngle=360+AngleZ-AngleQ;
		if (Typ==2)
				ZAngle=360-ZAngle;
		if (ZAngle>180)
		{
			if (r1>0)
			{
				yc=yc1+yq;
			}
		}
		else
		{
			if (r1<0)
			{
				yc=yc1+yq;
			}
		}
	}
}

void GNode::CirCenter(vector<Coord> &Value)
{
	float xq,yq,xz,yz,r;
	int Typ;
	float xc,yc;
	for (int i=1;i<Value.size();i++)
	{
		xc=0,yc=0;
		Typ=Value[i].Typ;
		if (Typ==2 || Typ==3)
		{
			xq=Value[i-1].x;
			yq=Value[i-1].y;
			xz=Value[i].x;
			yz=Value[i].y;
			r=Value[i].r;
			CalCenter(xq,yq,xz,yz,r,Typ,xc,yc);
		}
		Value[i].xc=xc;
		Value[i].yc=yc;
	}
}

vector<Coord> GNode::generateProfileCurve()
{
	map<string,float> Comp;
	vector<Coord> TValue;
	Coord Value;
	string row,tmp;
	char ch;
	bool addx,addy,addz,addI,addJ,addr;;
	float Axial;		
	int AxialComp;	
	int RadialComp;		
	float Radius;	
	int AR;
	int Typ;
	float x,y,z,r,xc,yc;
	float I,J;
	clearfirstspace(file);
	x=0;y=0;z=0;r=0;I=0;J=0;
	Radius=0;Axial=0;
	AxialComp=-1;RadialComp=-1;AR=1,Typ=0;
	for (int i=0;i<file.size();i++)
	{
		addx=false;addy=false;addz=false;addI=false;addJ=false;addr=false;
		I=0;J=0;r=0,xc=0,yc=0;
		row=file[i];
		int j=0;
		while (j < row.length())
		{
			ch=row[j];
			if (ch != ' ')
			{
				if (ch=='N')
				{
					while (ch != ' ' && ch !=';')
					{
						j++;
						ch=row[j];
					}
				}
				else if (ch=='G')
				{
					j++;
					tmp=row.substr(j,2);
					j=j+2;
					if (tmp=="00")
						Typ=0;
					else if (tmp=="01")
						Typ=1;
					else if (tmp=="02")
						Typ=2;
					else if (tmp=="03")
						Typ=3;
					else if (tmp=="90")
						AR=1;
					else if (tmp=="91")
						AR=2;
					else if (tmp=="43")
						AxialComp=1;
					else if (tmp=="44")
						AxialComp=2;
					else if (tmp=="49")
						AxialComp=0;
					else if (tmp=="40")
						RadialComp=0;
					else if (tmp=="41")
						RadialComp=1;
					else if (tmp=="42")
						RadialComp=2;
				}
				else if (ch=='X')
				{
					addx=true;
					getNumber(row,x,j);
				}
				else if (ch=='Y')
				{
					addy=true;
					getNumber(row,y,j);
				}
				else if (ch=='Z')
				{
					addz=true;
					getNumber(row,z,j);
				}
				else if (ch=='I')
				{
					addI=true;
					getNumber(row,I,j);
				}
				else if (ch=='J')
				{
					addJ=true;
					getNumber(row,J,j);
				}
				else if (ch=='R')
				{
					addr=true;
					getNumber(row,r,j);
				}
				else 
				{
					if (row[0] != 'N')
					{
						tmp="";
						while (j<row.size())
						{
							ch=row[j];
							if (ch=='=')
								break;
							if (ch != ' ')
								tmp=tmp+ch;	
							j++;
						}
						if (row[j]=='=')
						{
							if (row[j+1]==' ')
							{
								while (row[j+1]==' ')
									j++;
							}	
							float temp;
							getNumber(row,temp,j);
							Comp[tmp]=temp;
						}
					}
					else
					{
						tmp="";
						while (row[j] != ' ' && row[j] != ';')
						{
							tmp=tmp+row[j];	
							j++;	
						}
						int found=Comp.count(tmp);
						if (found==1)
						{
							if (AxialComp!=-1)
								Axial=Comp[tmp];
							else if (RadialComp!=-1)
								Radius=Comp[tmp];
						}
					}
				}
			}
			j++;
		}
		if (!((!addx) && (!addy) && (!addz)))
		{

			if (!TValue.empty())
			{
				if (!addx)
					x=TValue.back().x;
				if (!addy)
					y=TValue.back().y;
				if (!addz)
					z=TValue.back().z;
				if (!addI)
					I=TValue.back().I;
				if (!addJ)
					J=TValue.back().J;
				if (!addr)
					r=TValue.back().r;
			}
			Value.x=x;Value.y=y;Value.z=z;Value.I=I;Value.J=J;Value.r=r;
			Value.xc=xc;Value.yc=yc;
			Value.Typ=Typ;Value.AR=AR;
			Value.Axial=Axial;Value.AxialComp=AxialComp;
			Value.Radius=Radius;Value.RadialComp=RadialComp;
			TValue.push_back(Value);
		}
	}
	TValue=RtoA(TValue);
	AComp(TValue);
	IJtoR(TValue);
	CirCenter(TValue);
	//CValue=Compensate(TValue);
	ProfileCurve=TValue;
	return  ProfileCurve;
}



vector<Coord> GNode::generateContoutLine()
{
	ProfileCurve=generateProfileCurve();
	ContoutLine=Compensate(ProfileCurve);
	return ContoutLine;
}