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;
}
double pmCircle9Target(PmCircle9 const * const circ9)
{
double h2;
pmCartMagSq(&circ9->xyz.rHelix, &h2);
double helical_length = pmSqrt(pmSq(circ9->fit.total_planar_length) + h2);
return helical_length;
}
/**
* compute the total arc length of a circle segment
*/
int tcUpdateTargetFromCircle(TC_STRUCT * const tc)
{
if (!tc || tc->motion_type !=TC_CIRCULAR) {
return TP_ERR_FAIL;
}
double h2;
pmCartMagSq(&tc->coords.circle.xyz.rHelix, &h2);
double helical_length = pmSqrt(pmSq(tc->coords.circle.fit.total_planar_length) + h2);
tc->target = helical_length;
return TP_ERR_OK;
}
int pmCartInvEq(PmCartesian * const v)
{
double size_sq;
pmCartMagSq(v,&size_sq);
if (size_sq == 0.0) {
#ifdef PM_PRINT_ERROR
pmPrintError(&"Zero vector in pmCartInv\n");
#endif
return pmErrno = PM_NORM_ERR;
}
v->x /= size_sq;
v->y /= size_sq;
v->z /= size_sq;
return pmErrno = 0;
}
