int pmCartUnit(PmCartesian const * const v, PmCartesian * const vout) { if (vout != v) { *vout = *v; } return pmCartUnitEq(vout); }
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 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); }
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; }
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; }