PmCartesian tcGetStartingUnitVector(TC_STRUCT *tc) {
PmCartesian v;
if(tc->motion_type == TC_LINEAR || tc->motion_type == TC_RIGIDTAP) {
pmCartCartSub(tc->coords.line.xyz.end.tran, tc->coords.line.xyz.start.tran, &v);
} else {
PmPose startpoint;
PmCartesian radius;
PmCartesian tan, perp;
pmCirclePoint(&tc->coords.circle.xyz, 0.0, &startpoint);
pmCartCartSub(startpoint.tran, tc->coords.circle.xyz.center, &radius);
pmCartCartCross(tc->coords.circle.xyz.normal, radius, &tan);
pmCartUnit(tan, &tan);
pmCartCartSub(tc->coords.circle.xyz.center, startpoint.tran, &perp);
pmCartUnit(perp, &perp);
pmCartScalMult(tan, tc->maxaccel, &tan);
pmCartScalMult(perp, pmSq(0.5 * tc->reqvel)/tc->coords.circle.xyz.radius, &perp);
pmCartCartAdd(tan, perp, &v);
}
pmCartUnit(v, &v);
return v;
}
PmCartesian tcGetUnitCart(TC_STRUCT *tc)
{
PmPose currentPose;
PmCartesian radialCart;
static const PmCartesian fake= {1.0,0.0,0.0};
if(tc->type == TC_LINEAR)
{
pmCartCartSub(tc->line.end.tran,tc->line.start.tran,&tc->unitCart);
#ifdef USE_PM_CART_NORM
pmCartNorm(tc->unitCart,&tc->unitCart);
#else
pmCartUnit(tc->unitCart,&tc->unitCart);
#endif
return(tc->unitCart);
}
else if(tc->type == TC_CIRCULAR)
{
pmCirclePoint(&tc->circle,tc->currentPos,¤tPose);
pmCartCartSub(currentPose.tran, tc->circle.center, &radialCart);
pmCartCartCross(tc->circle.normal, radialCart,&tc->unitCart);
#ifdef USE_PM_CART_NORM
pmCartNorm(tc->unitCart,&tc->unitCart);
#else
pmCartUnit(tc->unitCart,&tc->unitCart);
#endif
return(tc->unitCart);
}
// It should never really get here.
return fake;
}
int pmCartLineInit(PmCartLine * const line, PmCartesian const * const start, PmCartesian const * const end)
{
int r1 = 0, r2 = 0;
double tmag = 0.0;
if (0 == line) {
return (pmErrno = PM_ERR);
}
line->start = *start;
line->end = *end;
r1 = pmCartCartSub(end, start, &line->uVec);
if (r1) {
return r1;
}
pmCartMag(&line->uVec, &tmag);
if (IS_FUZZ(tmag, CART_FUZZ)) {
line->uVec.x = 1.0;
line->uVec.y = 0.0;
line->uVec.z = 0.0;
} else {
r2 = pmCartUnit(&line->uVec, &line->uVec);
}
line->tmag = tmag;
line->tmag_zero = (line->tmag <= CART_FUZZ);
/* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */
return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
}
int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end)
{
PmCartesian r_perp;
PmCartesian r_tan;
if (at_end) {
r_perp = arc->rEnd;
} else {
r_perp = arc->rStart;
}
pmCartCartCross(&arc->binormal, &r_perp, &r_tan);
//Get spiral component
double dr = arc->spiral / arc->angle;
//Get perpendicular component due to spiral
PmCartesian d_perp;
pmCartUnit(&r_perp, &d_perp);
pmCartScalMultEq(&d_perp, dr);
//TODO error checks
pmCartCartAdd(&d_perp, &r_tan, tan);
pmCartUnitEq(tan);
return TP_ERR_OK;
}
/**
* Find the geometric tangent vector to a helical arc.
* Unlike the acceleration vector, the result of this calculation is a vector
* tangent to the helical arc. This is called by wrapper functions for the case of a circular or helical arc.
*/
int pmCircleTangentVector(PmCircle const * const circle,
double angle_in, PmCartesian * const out)
{
PmCartesian startpoint;
PmCartesian radius;
PmCartesian uTan, dHelix, dRadial;
// Get vector in radial direction
pmCirclePoint(circle, angle_in, &startpoint);
pmCartCartSub(&startpoint, &circle->center, &radius);
/* Find local tangent vector using planar normal. Assuming a differential
* angle dtheta, the tangential component of the tangent vector is r *
* dtheta. Since we're normalizing the vector anyway, assume dtheta = 1.
*/
pmCartCartCross(&circle->normal, &radius, &uTan);
// find dz/dtheta and get differential movement along helical axis
double h;
pmCartMag(&circle->rHelix, &h);
/* the binormal component of the tangent vector is (dz / dtheta) * dtheta.
*/
double dz = 1.0 / circle->angle;
pmCartScalMult(&circle->rHelix, dz, &dHelix);
pmCartCartAddEq(&uTan, &dHelix);
/* The normal component is (dr / dtheta) * dtheta.
*/
double dr = circle->spiral / circle->angle;
pmCartUnit(&radius, &dRadial);
pmCartScalMultEq(&dRadial, dr);
pmCartCartAddEq(&uTan, &dRadial);
//Normalize final output vector
pmCartUnit(&uTan, out);
return 0;
}
int pmLineInit(PmLine * const line, PmPose const * const start, PmPose const * const end)
{
int r1 = 0, r2 = 0, r3 = 0, r4 = 0, r5 = 0;
double tmag = 0.0;
double rmag = 0.0;
PmQuaternion startQuatInverse;
if (0 == line) {
return (pmErrno = PM_ERR);
}
r3 = pmQuatInv(&start->rot, &startQuatInverse);
if (r3) {
return r3;
}
r4 = pmQuatQuatMult(&startQuatInverse, &end->rot, &line->qVec);
if (r4) {
return r4;
}
pmQuatMag(&line->qVec, &rmag);
if (rmag > Q_FUZZ) {
r5 = pmQuatScalMult(&line->qVec, 1 / rmag, &(line->qVec));
if (r5) {
return r5;
}
}
line->start = *start;
line->end = *end;
r1 = pmCartCartSub(&end->tran, &start->tran, &line->uVec);
if (r1) {
return r1;
}
pmCartMag(&line->uVec, &tmag);
if (IS_FUZZ(tmag, CART_FUZZ)) {
line->uVec.x = 1.0;
line->uVec.y = 0.0;
line->uVec.z = 0.0;
} else {
r2 = pmCartUnit(&line->uVec, &line->uVec);
}
line->tmag = tmag;
line->rmag = rmag;
line->tmag_zero = (line->tmag <= CART_FUZZ);
line->rmag_zero = (line->rmag <= Q_FUZZ);
/* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */
return pmErrno = (r1 || r2 || r3 || r4 || r5) ? PM_NORM_ERR : 0;
}
/*
pmCirclePoint() returns the point at the given angle along
the circle. If the circle is a helix or spiral or combination, the
point will include interpolation off the actual circle.
*/
int pmCirclePoint(PmCircle const * const circle, double angle, PmCartesian * const point)
{
PmCartesian par, perp;
double scale;
#ifdef PM_DEBUG
if (0 == circle || 0 == point) {
#ifdef PM_PRINT_ERROR
pmPrintError
("error: pmCirclePoint circle or point pointer is null\n");
#endif
return pmErrno = PM_ERR;
}
#endif
/* compute components rel to center */
pmCartScalMult(&circle->rTan, rtapi_cos(angle), &par);
pmCartScalMult(&circle->rPerp, rtapi_sin(angle), &perp);
/* add to get radius vector rel to center */
pmCartCartAdd(&par, &perp, point);
/* get scale for spiral, helix interpolation */
if (circle->angle == 0.0) {
#ifdef PM_PRINT_ERROR
pmPrintError("error: pmCirclePoint angle is zero\n");
#endif
return pmErrno = PM_DIV_ERR;
}
scale = angle / circle->angle;
/* add scaled vector in radial dir for spiral */
pmCartUnit(point, &par);
pmCartScalMult(&par, scale * circle->spiral, &par);
pmCartCartAdd(point, &par, point);
/* add scaled vector in helix dir */
pmCartScalMult(&circle->rHelix, scale, &perp);
pmCartCartAdd(point, &perp, point);
/* add to center vector for final result */
pmCartCartAdd(&circle->center, point, point);
return pmErrno = 0;
}
PmCartesian tcGetEndingUnitVector(TC_STRUCT *tc) {
PmCartesian v;
if(tc->motion_type == TC_LINEAR) {
pmCartCartSub(tc->coords.line.xyz.end.tran, tc->coords.line.xyz.start.tran, &v);
} else if(tc->motion_type == TC_RIGIDTAP) {
// comes out the other way
pmCartCartSub(tc->coords.line.xyz.start.tran, tc->coords.line.xyz.end.tran, &v);
} else {
PmPose endpoint;
PmCartesian radius;
pmCirclePoint(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, &endpoint);
pmCartCartSub(endpoint.tran, tc->coords.circle.xyz.center, &radius);
pmCartCartCross(tc->coords.circle.xyz.normal, radius, &v);
}
pmCartUnit(v, &v);
return v;
}
/*
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;
}
