int pmCartScalMult(PmCartesian const * const v1, double d, PmCartesian * const vout)
{
if (v1 != vout) {
*vout = *v1;
}
return pmCartScalMultEq(vout, d);
}
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);
}
/**
* 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;
}
