agl.cc

数控系统中的解释器源代码

#include "agl.hh"
#include <math.h>
#define   ZERO    0.000001
#define M_PI_180          0.01745329251994

/********************************************************************
 *  By a Point and a Angle to set up  Coordinates of the new Origin
 *
 *  p0      represents a Point(x,y)
 *  aux     represents a Angle
 *  origin  new Coordinates
 *
 ********************************************************************/
void   CNC_AGLSetNewOrigin(PointVar        *p0,
				    AuxInf          *aux,
				    OriginVar       *origin)
{
double      tmpvalue;

	tmpvalue     = M_PI_180 * aux->a;
	origin->Sin  = sin(tmpvalue);
	origin->Cos  = cos(tmpvalue);
        origin->PosX = p0->PosX;
        origin->PosY = p0->PosY;
        origin->Angle = aux->a;
}

/********************************************************************
 *  Transformation of a point in specified Coordinates System
 *
 *  p0      represent the original point
 *  origin  represent a coordinates system
 *  p       represent the result of transformation of a point
 *  Ex      exception code; 8 == invalid coordinats system
 ********************************************************************/
void   CNC_AGLpTransformation(PointVar        *p0,
				       OriginVar       *origin,
				       PointVar        *p,
				       int             *Ex)
{
        if (origin->Sin == 0.0 && origin->Cos == 0.0)
          *Ex = 0x0008;
        else
          {
	  p->PosX = origin->PosX + origin->Cos * p0->PosX +
		      origin->Sin * p0->PosY;
	  p->PosY = origin->PosY - origin->Sin * p0->PosX +
                      origin->Cos * p0->PosY;
          }
}


/********************************************************************
 *  Transformation a point from  polar to cartesian
 *
 *  ang     represents a point in polar
 *  p       represents a point in cartesian
 *
 *
 ********************************************************************/
/*
void   CNC_AGLPolarToCartesian(PolarInf           *ang,
					PointVar           *p)
{
double    tmpvalue,tmp;

	tmpvalue = ang->a * M_PI_180;
	tmp	 = ang->m;
	p->PosX = tmp * cos(tmpvalue);
	p->PosY = tmp * sin(tmpvalue);
}
*/

/********************************************************************
 *  get intersection point of two given lines
 *
 *  l0,l1   two given lines
 *  p       intersection point
 *  MinValue  a unit to represents a minimum value
 *  Ex      except code; 1 == no intersection point(parallel)
 ********************************************************************/
void   CNC_AGLPointOf2Line(LineVar            *l0,
				    LineVar            *l1,
				    PointVar           *p,
				    float              MinValue,
				    int                *Ex)
{
double    tmpvalue;

	tmpvalue = l1->PlusCos * l0->MinusSin - l1->MinusSin * l0->PlusCos;
	if (fabs(tmpvalue) <= ZERO )
	  *Ex = 0x0001;
	else
	  {
	  p->PosX = (l1->Distance*l0->PlusCos - l1->PlusCos*l0->Distance)/
                      tmpvalue;
          p->PosY = (l0->Distance*l1->MinusSin - l1->Distance*l0->MinusSin)/
                      tmpvalue;
	  }
}



/********************************************************************
 *  get intersection point of a given line and a given circle
 *
 *  l0,c0   given AGL elements
 *  s       given intersection no
 *  p       intersection point value
 *  Ret     return information; 1 == only one intersection point
 *  Ex      exception code; 1 == no intersection point
 ********************************************************************/
void   CNC_AGLPointOfLineArc(LineVar            *l0,
				      CircleVar          *c0,
				      int                *s,
				      PointVar           *p,
				      float              MinValue,
				      int                *Ret,
				      int                *Ex)
{
int     sign;
double  tmp0,tmp1,tmp2;

	if (*s)
	  sign = -1;
        else
          sign =  1;

        // compute the distance (circle heart to the line)

        tmp0 = c0->HeartX*l0->MinusSin + c0->HeartY*l0->PlusCos + l0->Distance;
        tmp1 = fabs(c0->Radius) -  fabs(tmp0);

        if ( fabs(tmp1) <= MinValue)
          {
          p->PosX = c0->HeartX - l0->MinusSin * tmp0;
          p->PosY = c0->HeartY - l0->PlusCos   * tmp0;
          }
        else if (tmp1 > 0.0)
          {
	  tmp2 = c0->Radius * c0->Radius - tmp0 * tmp0;
	  tmp2 = sqrt(tmp2) * sign;
	  p->PosX = c0->HeartX - l0->MinusSin * tmp0 - l0->PlusCos * tmp2;
	  p->PosY = c0->HeartY - l0->PlusCos  * tmp0 + l0->MinusSin * tmp2;
	  }
	else
	  *Ex = 0x0001;
}



/********************************************************************
 *  get the intersection point of two given circles
 *
 *  c0,c1   two given circles
 *  s       given intersection no
 *  p       intersection point
 *  Ret     return information;
 *          1 == only one intersection point
 *          2 == two given circles are the same
 *  Ex      exception code;
 *          1 == no intersection point
 *          2 == two given circles are the same
 ********************************************************************/
void   CNC_AGLPointOf2Arc(CircleVar            *c0,
				   CircleVar            *c1,
				   int                  *s,
				   PointVar             *p,
				   float                MinValue,
				   int                  *Ret,
				   int                  *Ex)
{
LineVar     l;
double      yvar,xvar,distance,rsum,rvar,xm,ym,rm1,rm2;
int         internum;

        *Ret = *Ex = 0;
        xvar = c1->HeartX - c0->HeartX;
        yvar = c1->HeartY - c0->HeartY;

        distance = xvar * xvar + yvar * yvar;

        distance = sqrt(distance); /* get the distance between 2 heart */

	if (distance <= MinValue)    /* is the same circle ? */
	  {
          *Ex = *Ret = 2;
          return;
          }

        rsum = fabs(c0->Radius) + fabs(c1->Radius); /* sum of radius */
        rvar = distance - rsum;
        internum = 0;
        if (rvar > MinValue)   /* no intersection ? */
          {
          *Ex = 1;
          return;
	  }

        if (rvar >= - MinValue)  /* two circles are tangent ? */
          {
          *Ret = 0x0001;
          internum = 1;
          }
	else if (c1->Radius > c0->Radius) /* who is the bigger circle */
	  rvar =  fabs(c1->Radius) - (distance +  fabs(c0->Radius)) ;
        else
          rvar =  fabs(c0->Radius) - (distance +  fabs(c1->Radius)) ;

        if (!internum && rvar > MinValue)  /* small within bigger and no inter */
          *Ex = 0x0001;
        else
          {
          l.MinusSin = xvar / distance;
          l.PlusCos  = yvar / distance;
	  xm  = (c0->HeartX + c1->HeartX) / 2;
	  ym  = (c0->HeartY + c1->HeartY) / 2;
	  rm1 = (c1->Radius - c0->Radius) / distance;
	  rm2 = (c1->Radius + c0->Radius) / 2;
	  l.Distance = rm1 * rm2 - l.MinusSin * xm - l.PlusCos * ym;
	  CNC_AGLPointOfLineArc(&l,
				c0,
				s,
				p,
				MinValue,
				Ret,
				Ex);
          if (*Ex)
            *Ex = 0x0001;
          }
}


/********************************************************************
 *  get a line tangent to a given circle and form a angle with abscissa
 *
 *  c0    the given circle
 *  aux   contain the angle by abscissa
 *  l     the desired line
 ********************************************************************/
void   CNC_AGLTLineOfArcArg(CircleVar        *c0,
				     AuxInf           *aux,
				     LineVar          *l)
{
double      tmp;

        tmp = M_PI_180 * aux->a;
	l->MinusSin = - sin(tmp);
	l->PlusCos  =   cos(tmp);
        l->Distance = c0->Radius -
                      (l->MinusSin*c0->HeartX + l->PlusCos*c0->HeartY);
}


/********************************************************************
 *  get a line passing through two given points
 *
 *  p0,p1     the given points
 *  l         the desired line
 *  Ex        except code; 1 == two points are the same
 *
 ********************************************************************/
void   CNC_AGLLineOf2Point(PointVar           *p0,
				    PointVar           *p1,
				    LineVar            *l,
				    float              MinValue,
				    int                *Ex)
{
double  xvar,yvar,distance;

        *Ex = 0;
        xvar = p1->PosX - p0->PosX;
        yvar = p1->PosY - p0->PosY;

        distance = xvar * xvar + yvar * yvar;
        distance = sqrt(distance);

        if (distance > MinValue)   /* the same point ? */
          {
          l->MinusSin = -yvar / distance;
          l->PlusCos  = xvar / distance;
	  l->Distance = -(l->MinusSin * p0->PosX + l->PlusCos * p0->PosY);
	  }
	else
	  *Ex = 0x0002;
}



/********************************************************************
 *  Get a line tangent to two given circles
 *
 *  c0,c1   the two given circles
 *  l       the desired line
 *  Ex      except code; 6 == no such a solution
 *
 ********************************************************************/
void   CNC_AGLTLineOf2Arc(CircleVar            *c0,
				   CircleVar            *c1,
				   LineVar              *l,
				   float                MinValue,
				   int                  *Ex)
{
double  xvar,yvar,rvar,tmp0,tmp1,tmp2;

        *Ex = 0;
        xvar = c1->HeartX - c0->HeartX;
        yvar = c1->HeartY - c0->HeartY;
        rvar = c1->Radius - c0->Radius;

        tmp0 = xvar * xvar + yvar * yvar;
        tmp1 = tmp0 - rvar * rvar;

        if (tmp1 > MinValue) /* exist tangent line ? */
          {
          tmp2 = sqrt(tmp1);
          l->MinusSin = -(tmp2 * yvar - rvar * xvar) / tmp0;
	  l->PlusCos  =  (tmp2 * xvar + rvar * yvar) / tmp0;
	  l->Distance = -(l->MinusSin * c0->HeartX + l->PlusCos * c0->HeartY )+
			c0->Radius;
          }
	else
          *Ex = 0x0006;
}


/********************************************************************
 *  get a circle tagnent to two lines and have a known radius
 *
 *  l0,l1   the two given lines
 *  c       the desired circle
 *  Ex      exception code;
 *
 ********************************************************************/
void   CNC_AGLTArcOf2LineR(LineVar            *l0,
				    LineVar            *l1,
				    AuxInf             *aux,
				    CircleVar          *c,
				    float              MinValue,
				    int                *Ex)
{
double  rx,ry,tmp;

        *Ex = 0;
        tmp = l1->PlusCos * l0->MinusSin - l1->MinusSin * l0->PlusCos;
        if (fabs(tmp) > MinValue) /* two lines are parallel ? */
          {
          rx = l0->Distance - aux->r;
          ry = l1->Distance - aux->r;

          c->HeartX = (l0->PlusCos  * ry - l1->PlusCos  * rx) / tmp;
	  c->HeartY = (l1->MinusSin * rx - l0->MinusSin * ry) / tmp;
          c->Radius = aux->r;
	  }
	else
	  *Ex = 0x0005;
}


/********************************************************************
 *  Get a circle with a known radius,
 *  tangent to a given line and a given circle.
 *
 *  c0    the given circle
 *  l0    the given line
 *  aux   contains the known radius
 *  c     the desired circle
 *  Ex    exception code;
 ********************************************************************/
void   CNC_AGLTArcOfArcLineR(CircleVar            *c0,
				      LineVar              *l0,
				      AuxInf               *aux,
				      CircleVar            *c,
				      float                MinValue,
				      int                  *Ex)
{
LineVar     l;
CircleVar   ctmp;
PointVar    p;
int         s, ret;
double      tmp;


        s = 0;

        ctmp.HeartX = c0->HeartX;
        ctmp.HeartY = c0->HeartY;