agl.cc

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

        ctmp.Radius =  c0->Radius - aux->r;

        l.MinusSin = l0->MinusSin;
        l.PlusCos  = l0->PlusCos;
	l.Distance  =  l0->Distance - aux->r;


	if (c0->Radius != 0.0)
          {
          tmp  = l0->Distance +
                 l0->MinusSin * c0->HeartX + l0->PlusCos * c0->HeartY;
	  tmp =  fabs(tmp) -  fabs(c0->Radius);

          if (tmp >=  MinValue || aux->r * ctmp.Radius< 0)
            s = 1;
          }
        else
          s = 1;
	CNC_AGLPointOfLineArc(&l,
			      &ctmp,
			      &s,
			      &p,
			      MinValue,
			      &ret,
			      Ex);
	if (!*Ex)
	  {
	  c->HeartX = p.PosX;
          c->HeartY = p.PosY;
          c->Radius = aux->r;
          }
	else
	  *Ex = 0x0006;
}



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


	s = -1;

	ctmp.HeartX = c0->HeartX;
        ctmp.HeartY = c0->HeartY;
        ctmp.Radius =  c0->Radius - aux->r;

        l.MinusSin = l0->MinusSin;
        l.PlusCos  = l0->PlusCos;
        l.Distance  =  l0->Distance - aux->r;

        if (c0->Radius != 0.0)
	  {
          tmp  = l0->Distance +
                 l0->MinusSin * c0->HeartX + l0->PlusCos * c0->HeartY;
	  tmp =  fabs(tmp) -  fabs(c0->Radius);

	  if (tmp >=  MinValue || aux->r * ctmp.Radius < 0)
	    s = 0;
	  }
	else
	  s = 0;
	CNC_AGLPointOfLineArc(&l,
			      &ctmp,
			      &s,
			      &p,
			      MinValue,
			      &ret,
			      Ex);
	if (!*Ex)
	  {
	  c->HeartX = p.PosX;
	  c->HeartY = p.PosY;
	  c->Radius = aux->r;
	  }
	else
	  *Ex = 0x0006;
}

/********************************************************************
 *  get a circle tangent to two given circles
 *
 *  c0,c1   the two given circles
 *  aux     contains nothing
 *  c       the desired circle
 *  Ex      exception code;
 *          2 == two given circles are the same
 *          6 == no solution exist
 ********************************************************************/
void   CNC_AGLTArcOf2Arc(CircleVar            *c0,
				  CircleVar            *c1,
				  AuxInf               *aux,
				  CircleVar            *c,
				  float                MinValue,
				  int                  *Ex)
{
int           s,ret;
PointVar      p;
double        xvar,yvar,distance,c0radius,c1radius,radius;
double        rsum,oldc0radius,oldc1radius;

	yvar = c1->HeartX - c0->HeartX;
	xvar = c1->HeartY - c0->HeartY;

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

        if (distance <= MinValue)
	  {
          *Ex = 0x0006;
          return;
          }
        c0radius = c0->Radius - aux->r;
        c1radius = c1->Radius - aux->r;
        radius = c0->Radius * c1->Radius;

	if (radius == 0.0)
          {
          radius = 1.0;
          if (c0->Radius == 0.0)
            radius = c0->Radius;
          if (c1->Radius == 0.0)
            radius = c1->Radius;
          }
        s = 1;
        if (aux->r < 0.0)
          s = -1;
        oldc0radius =  fabs(c0->Radius);
	oldc1radius =  fabs(c1->Radius);
        rsum = oldc1radius + oldc0radius;
        if ((distance - rsum) > MinValue && radius < 0.0)
          s = ~s;
        else if ((distance - rsum) < MinValue)
          {
          if (oldc1radius > oldc0radius)
            {
	    distance += oldc0radius;
            rsum = oldc1radius;
            }
          else
            {
            distance += oldc1radius;
            rsum = oldc0radius;
            }
          if ((distance - rsum) <= MinValue && radius > 0.0)
            s = ~s;
          }
        oldc0radius = c0->Radius;
	oldc1radius = c1->Radius;

        c0->Radius = c0radius;
        c1->Radius = c1radius;

        if (s == 1)
          s = 0;
        else
	  s = 1;
	CNC_AGLPointOf2Arc(c0,
			   c1,
			   &s,
			   &p,
			   MinValue,
			   &ret,
			   Ex);
	c0->Radius = oldc0radius;
	c1->Radius = oldc1radius;

	if (!*Ex)
	  {
	  c->HeartX = p.PosX;
	  c->HeartY = p.PosY;
	  c->Radius = aux->r;
	  }
	else
	  *Ex = 0x0006;
}

/********************************************************************
 *  get a circle passing three given points
 *
 *  p0,p1,p2  the three given circles
 *  c         the desired circle
 *  Ex        exception code;
 *
 ********************************************************************/
void   CNC_AGLArcOf3Point(PointVar             *p0,
				   PointVar             *p1,
				   PointVar             *p2,
				   CircleVar            *c,
				   float                MinValue,
				   float                *MinDegree,
				   int                  *Ex)
{
PointVar    p;
LineVar     l1,l2,l3,l4;
double      xvalue,yvalue,tmp;
int         cdir;

	  CNC_AGLLineOf2Point(p0,
			      p1,
			      &l1,
			      MinValue,
			      Ex);      /* get a line */
	  if (!*Ex)
	    {
	    tmp = p2->PosX * l1.MinusSin + p2->PosY * l1.PlusCos + l1.Distance;

	    if ( fabs(tmp) > MinValue)
	      {
	      cdir = 1;
	      if (tmp < 0.0)
		cdir = -1;
	      CNC_AGLLineOf2Point(p1,
				  p2,
				  &l2,
				  MinValue,
				  Ex);      /* get a line */

	      xvalue = (p0->PosX + p1->PosX) / 2;   /* get mid point */
	      yvalue = (p0->PosY + p1->PosY) / 2;

	      l3.MinusSin = - l1.PlusCos;
	      l3.PlusCos  =   l1.MinusSin;
	      l3.Distance =  - (l3.MinusSin * xvalue + l3.PlusCos * yvalue);
	      xvalue = (p1->PosX + p2->PosX) / 2;
              yvalue = (p1->PosY + p2->PosY) / 2;

              l4.MinusSin = - l2.PlusCos;
              l4.PlusCos  =   l2.MinusSin;
              l4.Distance = - (l4.MinusSin * xvalue + l4.PlusCos * yvalue);

	      CNC_AGLPointOf2Line(&l3,
				  &l4,
				  &p,
				  *MinDegree,
				  Ex);
	      xvalue = p.PosX - p0->PosX;
	      yvalue = p.PosY - p0->PosY;
	      tmp = yvalue * yvalue + xvalue * xvalue;

	      c->Radius = sqrt(tmp) * cdir;
	      c->HeartX = p.PosX;
	      c->HeartY = p.PosY;

	      }
            else
              *Ex = 0x0007;
          }
}

/********************************************************************
 *  Get a circle,whose heart is the given point
 *  and tangent to a given line
 *
 *  p0      the given point
 *  l0      the given line
 *  c       the desired circle
 *
 ********************************************************************/
void   CNC_AGLTArcOfHeartLine(PointVar             *p0,
				       LineVar              *l0,
				       CircleVar            *c)
{

	c->HeartX = p0->PosX;
	c->HeartY = p0->PosY;
	c->Radius = p0->PosX * l0->MinusSin + p0->PosY * l0->PlusCos +
		    l0->Distance;
}

/********************************************************************
 *  get a circle,whose heart is the given point,and tangent to the given circle
 *
 *  p0    the given point
 *  c0    the given circle
 *  s     1 == new circel contain the old circle,that is the intersect point is  away
 *  c     the desired circle
 *
 ********************************************************************/
void   CNC_AGLTArcOfHeartArc(PointVar             *p0,
				      CircleVar            *c0,
				      int                  *s,
				      CircleVar            *c)
{
double  xvar,yvar,distance,radius;
int     sign;

	xvar = c0->HeartX - p0->PosX;
	yvar = c0->HeartY - p0->PosY;

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

	sign = 1;
        if (c0->Radius < 0.0)
          sign = -1;
        radius = fabs(c0->Radius);

	if ( !(*s) )      /* near tangent point ? */
	  if ((radius < distance))
            {
            radius = - radius;
            sign = ~sign;
            }
          else
            distance = - distance;
        c->Radius = (distance + radius ) * sign;
	c->HeartX = p0->PosX;
        c->HeartY = p0->PosY;
}

/********************************************************************
 *  get a line tangent to two given tangent circles
 *
 *  c0,c1   the two given circles
 *  l       the desired line
 *  Ex      exception code;
 *
 ********************************************************************/
void   CNC_AGLTLineOf2TArc(CircleVar            *c0,
				    CircleVar            *c1,
				    LineVar              *l,
				    float                MinValue,
				    int                  *Ex)
{
double    var0,var1,distance,rvar;

	  *Ex = 0;
	  var0 = c0->HeartX - c1->HeartX;
	  var1 = c0->HeartY - c1->HeartY;

	  distance = var0 * var0 + var1 * var1;   /* the distance between 2 hearts */

	  rvar = c0->Radius - c1->Radius;
          var0 = rvar * rvar - distance;
          var0 = fabs(var0);

          if (var0 <= MinValue)
	    {
            *Ex = 6;      /* two circle aren't intrernal tangent */
            return;
            }

	  CNC_AGLLineOf2Point((PointVar  *)c0,
			      (PointVar  *)c1,
			      l,
			      MinValue,
			      Ex);  /* two hearts get a new line */
	  if (!*Ex)
	    {
	    var1 = l->MinusSin;
	    if (c0->Radius < 0.0)   /* circle is CW */
	      {
	      l->MinusSin =   l->PlusCos;
	      l->PlusCos  = - var1;
	      }
	    else                    /* circle is CCW */
              {
	      l->MinusSin = - l->PlusCos;
              l->PlusCos = var1;
              }
            l->Distance = c0->Radius -
                        (c0->HeartX * l->MinusSin + c0->HeartY * l->PlusCos);
            }
}