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 pmCircleStretch(PmCircle * const circ, double new_angle, int from_end) { if (!circ || new_angle <= DOUBLE_FUZZ) { return PM_ERR; } double mag = 0; pmCartMagSq(&circ->rHelix, &mag); if ( mag > 1e-6 ) { //Can't handle helices return PM_ERR; } //TODO handle spiral? if (from_end) { //Not implemented yet, way more reprocessing... PmCartesian new_start; double start_angle = circ->angle - new_angle; pmCirclePoint(circ, start_angle, &new_start); pmCartCartSub(&new_start, &circ->center, &circ->rTan); pmCartCartCross(&circ->normal, &circ->rTan, &circ->rPerp); pmCartMag(&circ->rTan, &circ->radius); } //Reduce the spiral proportionally circ->spiral *= (new_angle / circ->angle); // Easy to grow / shrink from start circ->angle = new_angle; return 0; }
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; }
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; }
int pmMatIsNorm(PmRotationMatrix const * const m) { PmCartesian u; pmCartCartCross(&m->x, &m->y, &u); return (pmCartIsNorm(&m->x) && pmCartIsNorm(&m->y) && pmCartIsNorm(&m->z) && pmCartCartCompare(&u, &m->z)); }
int tcCircleEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { PmCartesian endpoint; PmCartesian radius; pmCirclePoint(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, &endpoint); pmCartCartSub(&endpoint, &tc->coords.circle.xyz.center, &radius); pmCartCartCross(&tc->coords.circle.xyz.normal, &radius, out); pmCartUnitEq(out); return 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; }
int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1, PmCartLine const * const line2, double radius, double blend_dist, double center_dist, PmCartesian * const start, PmCartesian * const end, int consume) { PmCartesian center, normal, binormal; // Pointer to middle point of line segment pair PmCartesian const * const middle = &line1->end; //TODO assert line1 end = line2 start? //Calculate the normal direction of the arc from the difference //between the unit vectors pmCartCartSub(&line2->uVec, &line1->uVec, &normal); pmCartUnitEq(&normal); pmCartScalMultEq(&normal, center_dist); pmCartCartAdd(middle, &normal, ¢er); //Calculate the binormal (vector perpendicular to the plane of the //arc) pmCartCartCross(&line1->uVec, &line2->uVec, &binormal); pmCartUnitEq(&binormal); // Start point is blend_dist away from middle point in the // negative direction of line1 pmCartScalMult(&line1->uVec, -blend_dist, start); pmCartCartAdd(start, middle, start); // End point is blend_dist away from middle point in the positive // direction of line2 pmCartScalMult(&line2->uVec, blend_dist, end); pmCartCartAddEq(end, middle); //Handle line portion of line-arc arc->uTan = line1->uVec; if (consume) { arc->line_length = line1->tmag - blend_dist; } else { arc->line_length = 0; } return arcInitFromPoints(arc, start, end, ¢er); }
/** * 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 tcCircleStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { PmCartesian startpoint; PmCartesian radius; PmCartesian tan, perp; pmCirclePoint(&tc->coords.circle.xyz, 0.0, &startpoint); pmCartCartSub(&startpoint, &tc->coords.circle.xyz.center, &radius); pmCartCartCross(&tc->coords.circle.xyz.normal, &radius, &tan); pmCartUnitEq(&tan); //The unit vector's actual direction is adjusted by the normal //acceleration here. This unit vector is NOT simply the tangent //direction. pmCartCartSub(&tc->coords.circle.xyz.center, &startpoint, &perp); pmCartUnitEq(&perp); pmCartScalMult(&tan, tc->maxaccel, &tan); pmCartScalMultEq(&perp, pmSq(0.5 * tc->reqvel)/tc->coords.circle.xyz.radius); pmCartCartAdd(&tan, &perp, out); pmCartUnitEq(out); 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; }