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;
}
/*
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;
}
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;
}
