Exemple #1
0
int tcSetCircle(TC_STRUCT *tc, PmCircle circle, PmLine line_abc)
{
  if (0 == tc)
  {
    return -1;
  }

  tc->circle = circle;
  tc->line_abc = line_abc;

  /* for circular motion, path param is arc length */
  tc->tmag = tc->targetPos = circle.angle * circle.radius;

  tc->currentPos = 0.0;
  tc->type = TC_CIRCULAR;

  tc->aMax = tc->taMax;
  tc->vMax = tc->tvMax;

  pmCartCartDisp(line_abc.end.tran, line_abc.start.tran, &tc->abc_mag);

  return 0;
}
Exemple #2
0
/*
  pmCircleInit() takes the defining parameters of a generalized circle
  and sticks them in the structure. It also computes the radius and vectors
  in the plane that are useful for other functions and that don't need
  to be recomputed every time.

  Note that the end can be placed arbitrarily, resulting in a combination of
  spiral and helical motion. There is an overconstraint between the start,
  center, and normal vector: the center vector and start vector are assumed
  to be in the plane defined by the normal vector. If this is not true, then
  it will be made true by moving the center vector onto the plane.
  */
int pmCircleInit(PmCircle * const circle,
        PmCartesian const * const start, PmCartesian const * const end,
        PmCartesian const * const center, PmCartesian const * const normal, int turn)
{
    double dot;
    PmCartesian rEnd;
    PmCartesian v;
    double d;
    int r1;

#ifdef PM_DEBUG
    if (0 == circle) {
#ifdef PM_PRINT_ERROR
        pmPrintError("error: pmCircleInit cirle pointer is null\n");
#endif
        return pmErrno = PM_ERR;
    }
#endif

    /* adjust center */
    pmCartCartSub(start, center, &v);
    r1 = pmCartCartProj(&v, normal, &v);
    if (PM_NORM_ERR == r1) {
        /* bad normal vector-- abort */
#ifdef PM_PRINT_ERROR
        pmPrintError("error: pmCircleInit normal vector is 0\n");
#endif
        return -1;
    }
    pmCartCartAdd(&v, center, &circle->center);

    /* normalize and redirect normal vector based on turns. If turn is less
       than 0, point normal vector in other direction and make turn positive, 
       -1 -> 0, -2 -> 1, etc. */
    pmCartUnit(normal, &circle->normal);
    if (turn < 0) {
        turn = -1 - turn;
        pmCartScalMult(&circle->normal, -1.0, &circle->normal);
    }

    /* radius */
    pmCartCartDisp(start, &circle->center, &circle->radius);

    /* vector in plane of circle from center to start, magnitude radius */
    pmCartCartSub(start, &circle->center, &circle->rTan);
    /* vector in plane of circle perpendicular to rTan, magnitude radius */
    pmCartCartCross(&circle->normal, &circle->rTan, &circle->rPerp);

    /* do rHelix, rEnd */
    pmCartCartSub(end, &circle->center, &circle->rHelix);
    pmCartPlaneProj(&circle->rHelix, &circle->normal, &rEnd);
    pmCartMag(&rEnd, &circle->spiral);
    circle->spiral -= circle->radius;
    pmCartCartSub(&circle->rHelix, &rEnd, &circle->rHelix);
    pmCartUnit(&rEnd, &rEnd);
    pmCartScalMult(&rEnd, circle->radius, &rEnd);

    /* Patch for error spiral end same as spiral center */
    pmCartMag(&rEnd, &d);
    if (d == 0.0) {
        pmCartScalMult(&circle->normal, DOUBLE_FUZZ, &v);
        pmCartCartAdd(&rEnd, &v, &rEnd);
    }
    /* end patch 03-mar-1999 Dirk Maij */

    /* angle */
    pmCartCartDot(&circle->rTan, &rEnd, &dot);
    dot = dot / (circle->radius * circle->radius);
    if (dot > 1.0) {
        circle->angle = 0.0;
    } else if (dot < -1.0) {
        circle->angle = PM_PI;
    } else {
        circle->angle = rtapi_acos(dot);
    }
    /* now angle is in range 0..PI . Check if cross is antiparallel to
       normal. If so, true angle is between PI..2PI. Need to subtract from
       2PI. */
    pmCartCartCross(&circle->rTan, &rEnd, &v);
    pmCartCartDot(&v, &circle->normal, &d);
    if (d < 0.0) {
        circle->angle = PM_2_PI - circle->angle;
    }

    if (circle->angle > -(CIRCLE_FUZZ) && circle->angle < (CIRCLE_FUZZ)) {
        circle->angle = PM_2_PI;
    }

    /* now add more angle for multi turns */
    if (turn > 0) {
        circle->angle += turn * 2.0 * PM_PI;
    }

    //Default to invalid
/* if 0'ed out while not debugging*/
#if 0
    printf("\n\n");
    printf("pmCircleInit:\n");
    printf(" \t start  : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	start->x, start->y, start->z);
    printf(" \t end    : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	end->x, end->y, end->z);
    printf(" \t center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	center->x, center->y, center->z);
    printf(" \t normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	normal->x, normal->y, normal->z);
    printf(" \t rEnd   : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	rEnd.x, rEnd.y, rEnd.z);
    printf(" \t turn=%d\n", turn);
    printf(" \t dot=%9.9f\n", dot);
    printf(" \t d=%9.9f\n", d);
    printf(" \t circle  \t{angle=%9.9f, radius=%9.9f, spiral=%9.9f}\n",
	circle->angle, circle->radius, circle->spiral);
    printf(" \t circle->normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	circle->normal.x, circle->normal.y, circle->normal.z);
    printf(" \t circle->center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	circle->center.x, circle->center.y, circle->center.z);
    printf(" \t circle->rTan : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	circle->rTan.x, circle->rTan.y, circle->rTan.z);
    printf(" \t circle->rPerp : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
	circle->rPerp.x, circle->rPerp.y, circle->rPerp.z);
    printf(" \t circle->rHelix : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
            circle->rHelix.x, circle->rHelix.y, circle->rHelix.z);
    printf("\n\n");
#endif

    return pmErrno = 0;
}
Exemple #3
0
int tcSetLine(TC_STRUCT *tc, PmLine line, PmLine line_abc)
{
  double tmag,abc_mag;

  if (0 == tc)
  {
    return -1;
  }

  tc->line = line;
  tc->line_abc = line_abc;


  /* set targetPos to be scalar difference */
  pmCartCartDisp(line.end.tran, line.start.tran, &tmag);
  pmCartCartDisp(line_abc.end.tran, line_abc.start.tran, &abc_mag);

  tc->abc_mag = abc_mag;
  tc->tmag = tmag;
  
  if(tc->abc_aMax <= 0.0 && tc->taMax > 0.0)
    {
      tc->abc_aMax = tc->taMax;
    }

  if(tc->abc_vMax <= 0.0 && tc->tvMax > 0.0)
    {
      tc->abc_vMax = tc->tvMax;
    }

  if(tc->tmag < 1e-6)
    {
      tc->aMax = tc->abc_aMax;
      tc->vMax = tc->abc_vMax;
      tc->targetPos = abc_mag;
    }
  else
    {
      if(0)
#if 0
      if(tc->abc_mag > 1e-6)
#endif
	{
	  if(tc->abc_aMax * tmag/abc_mag < tc->taMax)
	    {
	      tc->aMax = tc->abc_aMax *tmag/abc_mag;
	    }
	  else
	    {
	      tc->aMax = tc->taMax;
	    }      
	  if(tc->abc_vMax * tmag/abc_mag < tc->tvMax)
	    {
	      tc->vMax = tc->abc_vMax * tmag /abc_mag;
	    }
	  else
	    {
	      tc->vMax = tc->tvMax;
	    }
	}
      else
	{
	  tc->aMax = tc->taMax;
	  tc->vMax = tc->tvMax;
	}
      tc->targetPos = tmag;
    }
     
  tc->currentPos = 0.0;
  tc->type = TC_LINEAR;
  return 0;
}