int pmRotNorm(PmRotationVector const * const r, PmRotationVector * const rout)
{
double size;
size = pmSqrt(pmSq(r->x) + pmSq(r->y) + pmSq(r->z));
if (rtapi_fabs(r->s) < RS_FUZZ) {
rout->s = 0.0;
rout->x = 0.0;
rout->y = 0.0;
rout->z = 0.0;
return pmErrno = 0;
}
if (size == 0.0) {
#ifdef PM_PRINT_ERROR
pmPrintError("error: pmRotNorm size is zero\n");
#endif
rout->s = 0.0;
rout->x = 0.0;
rout->y = 0.0;
rout->z = 0.0;
return pmErrno = PM_NORM_ERR;
}
rout->s = r->s;
rout->x = r->x / size;
rout->y = r->y / size;
rout->z = r->z / size;
return pmErrno = 0;
}
int pmQuatNorm(PmQuaternion const * const q1, PmQuaternion * const qout)
{
double size = pmSqrt(pmSq(q1->s) + pmSq(q1->x) + pmSq(q1->y) + pmSq(q1->z));
if (size == 0.0) {
#ifdef PM_PRINT_ERROR
pmPrintError("Bad quaternion in pmQuatNorm\n");
#endif
qout->s = 1;
qout->x = 0;
qout->y = 0;
qout->z = 0;
return pmErrno = PM_NORM_ERR;
}
if (q1->s >= 0.0) {
qout->s = q1->s / size;
qout->x = q1->x / size;
qout->y = q1->y / size;
qout->z = q1->z / size;
return pmErrno = 0;
} else {
qout->s = -q1->s / size;
qout->x = -q1->x / size;
qout->y = -q1->y / size;
qout->z = -q1->z / size;
return pmErrno = 0;
}
}
int pmCartCartDisp(PmCartesian const * const v1, PmCartesian const * const v2,
double *d)
{
*d = pmSqrt(pmSq(v2->x - v1->x) + pmSq(v2->y - v1->y) + pmSq(v2->z - v1->z));
return pmErrno = 0;
}
int pmCylSphConvert(PmCylindrical const * const c, PmSpherical * const s)
{
s->theta = c->theta;
s->r = pmSqrt(pmSq(c->r) + pmSq(c->z));
s->phi = rtapi_atan2(c->z, c->r);
return pmErrno = 0;
}
int pmQuatRotConvert(PmQuaternion const * const q, PmRotationVector * const r)
{
double sh;
#ifdef PM_DEBUG
if (!pmQuatIsNorm(q)) {
#ifdef PM_PRINT_ERROR
pmPrintError("Bad quaternion in pmQuatRotConvert\n");
#endif
return pmErrno = PM_NORM_ERR;
}
#endif
if (r == 0) {
return (pmErrno = PM_ERR);
}
sh = pmSqrt(pmSq(q->x) + pmSq(q->y) + pmSq(q->z));
if (sh > QSIN_FUZZ) {
r->s = 2.0 * rtapi_atan2(sh, q->s);
r->x = q->x / sh;
r->y = q->y / sh;
r->z = q->z / sh;
} else {
r->s = 0.0;
r->x = 0.0;
r->y = 0.0;
r->z = 0.0;
}
return pmErrno = 0;
}
int pmRotMatConvert(PmRotationVector const * const r, PmRotationMatrix * const m)
{
double s, c, omc;
#ifdef PM_DEBUG
if (!pmRotIsNorm(r)) {
#ifdef PM_PRINT_ERROR
pmPrintError("Bad vector in pmRotMatConvert\n");
#endif
return pmErrno = PM_NORM_ERR;
}
#endif
sincos(r->s, &s, &c);
/* from space book */
m->x.x = c + pmSq(r->x) * (omc = 1 - c); /* omc = One Minus Cos */
m->y.x = -r->z * s + r->x * r->y * omc;
m->z.x = r->y * s + r->x * r->z * omc;
m->x.y = r->z * s + r->y * r->x * omc;
m->y.y = c + pmSq(r->y) * omc;
m->z.y = -r->x * s + r->y * r->z * omc;
m->x.z = -r->y * s + r->z * r->x * omc;
m->y.z = r->x * s + r->z * r->y * omc;
m->z.z = c + pmSq(r->z) * omc;
return pmErrno = 0;
}
int pmCartCylConvert(PmCartesian const * const v, PmCylindrical * const c)
{
c->theta = rtapi_atan2(v->y, v->x);
c->r = pmSqrt(pmSq(v->x) + pmSq(v->y));
c->z = v->z;
return pmErrno = 0;
}
int pmRotIsNorm(PmRotationVector const * const r)
{
if (rtapi_fabs(r->s) < RS_FUZZ ||
rtapi_fabs(pmSqrt(pmSq(r->x) + pmSq(r->y) + pmSq(r->z))) - 1.0 < UNIT_VEC_FUZZ)
{
return 1;
}
return 0;
}
int pmCartSphConvert(PmCartesian const * const v, PmSpherical * const s)
{
double _r;
s->theta = rtapi_atan2(v->y, v->x);
s->r = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z));
_r = pmSqrt(pmSq(v->x) + pmSq(v->y));
s->phi = rtapi_atan2(_r, v->z);
return pmErrno = 0;
}
double pmCircle9Target(PmCircle9 const * const circ9)
{
double helix_z_component; // z of the helix's cylindrical coord system
double helix_length;
pmCartMag(&circ9->xyz.rHelix, &helix_z_component);
double planar_arc_length = circ9->xyz.angle * circ9->xyz.radius;
helix_length = pmSqrt(pmSq(planar_arc_length) +
pmSq(helix_z_component));
return helix_length;
}
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 pmCartUnitEq(PmCartesian * const v)
{
double size = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z));
if (size == 0.0) {
#ifdef PM_PRINT_ERROR
pmPrintError("Zero vector in pmCartUnit\n");
#endif
return pmErrno = PM_NORM_ERR;
}
v->x /= size;
v->y /= size;
v->z /= size;
return pmErrno = 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;
}
int pmMatZyxConvert(PmRotationMatrix const * const m, PmEulerZyx * const zyx)
{
zyx->y = rtapi_atan2(-m->x.z, pmSqrt(pmSq(m->x.x) + pmSq(m->x.y)));
if (rtapi_fabs(zyx->y - (2 * PM_PI)) < ZYX_Y_FUZZ) {
zyx->z = 0.0;
zyx->y = (2 * PM_PI); /* force it */
zyx->x = rtapi_atan2(m->y.x, m->y.y);
} else if (rtapi_fabs(zyx->y + (2 * PM_PI)) < ZYX_Y_FUZZ) {
zyx->z = 0.0;
zyx->y = -(2 * PM_PI); /* force it */
zyx->x = -rtapi_atan2(m->y.z, m->y.y);
} else {
zyx->z = rtapi_atan2(m->x.y, m->x.x);
zyx->x = rtapi_atan2(m->y.z, m->z.z);
}
return pmErrno = 0;
}
int pmMatRpyConvert(PmRotationMatrix const * const m, PmRpy * const rpy)
{
rpy->p = rtapi_atan2(-m->x.z, pmSqrt(pmSq(m->x.x) + pmSq(m->x.y)));
if (rtapi_fabs(rpy->p - (2 * PM_PI)) < RPY_P_FUZZ) {
rpy->r = rtapi_atan2(m->y.x, m->y.y);
rpy->p = (2 * PM_PI); /* force it */
rpy->y = 0.0;
} else if (rtapi_fabs(rpy->p + (2 * PM_PI)) < RPY_P_FUZZ) {
rpy->r = -rtapi_atan2(m->y.z, m->y.y);
rpy->p = -(2 * PM_PI); /* force it */
rpy->y = 0.0;
} else {
rpy->r = rtapi_atan2(m->y.z, m->z.z);
rpy->y = rtapi_atan2(m->x.y, m->x.x);
}
return pmErrno = 0;
}
int pmMatZyzConvert(PmRotationMatrix const * const m, PmEulerZyz * const zyz)
{
zyz->y = rtapi_atan2(pmSqrt(pmSq(m->x.z) + pmSq(m->y.z)), m->z.z);
if (rtapi_fabs(zyz->y) < ZYZ_Y_FUZZ) {
zyz->z = 0.0;
zyz->y = 0.0; /* force Y to 0 */
zyz->zp = rtapi_atan2(-m->y.x, m->x.x);
} else if (rtapi_fabs(zyz->y - PM_PI) < ZYZ_Y_FUZZ) {
zyz->z = 0.0;
zyz->y = PM_PI; /* force Y to 180 */
zyz->zp = rtapi_atan2(m->y.x, -m->x.x);
} else {
zyz->z = rtapi_atan2(m->z.y, m->z.x);
zyz->zp = rtapi_atan2(m->y.z, -m->x.z);
}
return pmErrno = 0;
}
int pmQuatMatConvert(PmQuaternion const * const q, PmRotationMatrix * const m)
{
#ifdef PM_DEBUG
if (!pmQuatIsNorm(q)) {
#ifdef PM_PRINT_ERROR
pmPrintError("Bad quaternion in pmQuatMatConvert\n");
#endif
return pmErrno = PM_NORM_ERR;
}
#endif
/* from space book where e1=q->x e2=q->y e3=q->z e4=q->s */
m->x.x = 1.0 - 2.0 * (pmSq(q->y) + pmSq(q->z));
m->y.x = 2.0 * (q->x * q->y - q->z * q->s);
m->z.x = 2.0 * (q->z * q->x + q->y * q->s);
m->x.y = 2.0 * (q->x * q->y + q->z * q->s);
m->y.y = 1.0 - 2.0 * (pmSq(q->z) + pmSq(q->x));
m->z.y = 2.0 * (q->y * q->z - q->x * q->s);
m->x.z = 2.0 * (q->z * q->x - q->y * q->s);
m->y.z = 2.0 * (q->y * q->z + q->x * q->s);
m->z.z = 1.0 - 2.0 * (pmSq(q->x) + pmSq(q->y));
return pmErrno = 0;
}
void tcRunCycle(TP_STRUCT *tp, TC_STRUCT *tc, double *v, int *on_final_decel) {
double discr, maxnewvel, newvel, newaccel=0;
if(!tc->blending) tc->vel_at_blend_start = tc->currentvel;
discr = 0.5 * tc->cycle_time * tc->currentvel - (tc->target - tc->progress);
if(discr > 0.0) {
// should never happen: means we've overshot the target
newvel = maxnewvel = 0.0;
} else {
discr = 0.25 * pmSq(tc->cycle_time) - 2.0 / tc->maxaccel * discr;
newvel = maxnewvel = -0.5 * tc->maxaccel * tc->cycle_time +
tc->maxaccel * pmSqrt(discr);
}
if(newvel <= 0.0) {
// also should never happen - if we already finished this tc, it was
// caught above
newvel = newaccel = 0.0;
tc->progress = tc->target;
} else {
// constrain velocity
if(newvel > tc->reqvel * tc->feed_override)
newvel = tc->reqvel * tc->feed_override;
if(newvel > tc->maxvel) newvel = tc->maxvel;
// if the motion is not purely rotary axes (and therefore in angular units) ...
if(!(tc->motion_type == TC_LINEAR && tc->coords.line.xyz.tmag_zero && tc->coords.line.uvw.tmag_zero)) {
// ... clamp motion's velocity at TRAJ MAX_VELOCITY (tooltip maxvel)
// except when it's synced to spindle position.
if((!tc->synchronized || tc->velocity_mode) && newvel > tp->vLimit) {
newvel = tp->vLimit;
}
}
// get resulting acceleration
newaccel = (newvel - tc->currentvel) / tc->cycle_time;
// constrain acceleration and get resulting velocity
if(newaccel > 0.0 && newaccel > tc->maxaccel) {
newaccel = tc->maxaccel;
newvel = tc->currentvel + newaccel * tc->cycle_time;
}
if(newaccel < 0.0 && newaccel < -tc->maxaccel) {
newaccel = -tc->maxaccel;
newvel = tc->currentvel + newaccel * tc->cycle_time;
}
// update position in this tc
tc->progress += (newvel + tc->currentvel) * 0.5 * tc->cycle_time;
}
tc->currentvel = newvel;
if(v) *v = newvel;
if(on_final_decel) *on_final_decel = fabs(maxnewvel - newvel) < 0.001;
}
/**
* 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 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;
}
int tpAddCircle(TP_STRUCT * tp, EmcPose end,
PmCartesian center, PmCartesian normal, int turn, int type,
double vel, double ini_maxvel, double acc, unsigned char enables, char atspeed)
{
TC_STRUCT tc;
PmCircle circle;
PmLine line_uvw, line_abc;
PmPose start_xyz, end_xyz;
PmPose start_uvw, end_uvw;
PmPose start_abc, end_abc;
double helix_z_component; // z of the helix's cylindrical coord system
double helix_length;
PmQuaternion identity_quat = { 1.0, 0.0, 0.0, 0.0 };
if (!tp || tp->aborting)
return -1;
start_xyz.tran = tp->goalPos.tran;
end_xyz.tran = end.tran;
start_abc.tran.x = tp->goalPos.a;
start_abc.tran.y = tp->goalPos.b;
start_abc.tran.z = tp->goalPos.c;
end_abc.tran.x = end.a;
end_abc.tran.y = end.b;
end_abc.tran.z = end.c;
start_uvw.tran.x = tp->goalPos.u;
start_uvw.tran.y = tp->goalPos.v;
start_uvw.tran.z = tp->goalPos.w;
end_uvw.tran.x = end.u;
end_uvw.tran.y = end.v;
end_uvw.tran.z = end.w;
start_xyz.rot = identity_quat;
end_xyz.rot = identity_quat;
start_uvw.rot = identity_quat;
end_uvw.rot = identity_quat;
start_abc.rot = identity_quat;
end_abc.rot = identity_quat;
pmCircleInit(&circle, start_xyz, end_xyz, center, normal, turn);
pmLineInit(&line_uvw, start_uvw, end_uvw);
pmLineInit(&line_abc, start_abc, end_abc);
// find helix length
pmCartMag(circle.rHelix, &helix_z_component);
helix_length = pmSqrt(pmSq(circle.angle * circle.radius) +
pmSq(helix_z_component));
tc.sync_accel = 0;
tc.cycle_time = tp->cycleTime;
tc.target = helix_length;
tc.progress = 0.0;
tc.reqvel = vel;
tc.maxaccel = acc;
tc.feed_override = 0.0;
tc.maxvel = ini_maxvel;
tc.id = tp->nextId;
tc.active = 0;
tc.atspeed = atspeed;
tc.currentvel = 0.0;
tc.blending = 0;
tc.blend_vel = 0.0;
tc.vel_at_blend_start = 0.0;
tc.coords.circle.xyz = circle;
tc.coords.circle.uvw = line_uvw;
tc.coords.circle.abc = line_abc;
tc.motion_type = TC_CIRCULAR;
tc.canon_motion_type = type;
tc.blend_with_next = tp->termCond == TC_TERM_COND_BLEND;
tc.tolerance = tp->tolerance;
tc.synchronized = tp->synchronized;
tc.velocity_mode = tp->velocity_mode;
tc.uu_per_rev = tp->uu_per_rev;
tc.enables = enables;
tc.indexrotary = -1;
if (syncdio.anychanged != 0) {
tc.syncdio = syncdio; //enqueue the list of DIOs that need toggling
tpClearDIOs(); // clear out the list, in order to prepare for the next time we need to use it
} else {
tc.syncdio.anychanged = 0;
}
if (tcqPut(&tp->queue, tc) == -1) {
return -1;
}
tp->goalPos = end;
tp->done = 0;
tp->depth = tcqLen(&tp->queue);
tp->nextId++;
return 0;
}
/** Find square of magnitude of a vector (useful for some calculations to save a sqrt).*/
int pmCartMagSq(PmCartesian const * const v, double *d)
{
*d = pmSq(v->x) + pmSq(v->y) + pmSq(v->z);
return pmErrno = 0;
}
int pmCartIsNorm(PmCartesian const * const v)
{
return pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z)) - 1.0 < UNIT_VEC_FUZZ ? 1 : 0;
}
EmcPose tcGetPosReal(TC_STRUCT * tc, int of_endpoint)
{
EmcPose pos;
PmPose xyz;
PmPose abc;
PmPose uvw;
double progress = of_endpoint? tc->target: tc->progress;
#if(TRACE != 0)
static double last_l, last_u,last_x = 0 , last_y = 0, last_z = 0, last_a = 0;
#endif
if (tc->motion_type == TC_RIGIDTAP) {
if(tc->coords.rigidtap.state > REVERSING) {
pmLinePoint(&tc->coords.rigidtap.aux_xyz, progress, &xyz);
} else {
pmLinePoint(&tc->coords.rigidtap.xyz, progress, &xyz);
}
// no rotary move allowed while tapping
abc.tran = tc->coords.rigidtap.abc;
uvw.tran = tc->coords.rigidtap.uvw;
} else if (tc->motion_type == TC_LINEAR) {
if (tc->coords.line.xyz.tmag > 0.) {
// progress is along xyz, so uvw and abc move proportionally in order
// to end at the same time.
pmLinePoint(&tc->coords.line.xyz, progress, &xyz);
pmLinePoint(&tc->coords.line.uvw,
progress * tc->coords.line.uvw.tmag / tc->target,
&uvw);
pmLinePoint(&tc->coords.line.abc,
progress * tc->coords.line.abc.tmag / tc->target,
&abc);
} else if (tc->coords.line.uvw.tmag > 0.) {
// xyz is not moving
pmLinePoint(&tc->coords.line.xyz, 0.0, &xyz);
pmLinePoint(&tc->coords.line.uvw, progress, &uvw);
// abc moves proportionally in order to end at the same time
pmLinePoint(&tc->coords.line.abc,
progress * tc->coords.line.abc.tmag / tc->target,
&abc);
} else {
// if all else fails, it's along abc only
pmLinePoint(&tc->coords.line.xyz, 0.0, &xyz);
pmLinePoint(&tc->coords.line.uvw, 0.0, &uvw);
pmLinePoint(&tc->coords.line.abc, progress, &abc);
}
} else if (tc->motion_type == TC_CIRCULAR) {//we have TC_CIRCULAR
// progress is always along the xyz circle. This simplification
// is possible since zero-radius arcs are not allowed by the interp.
pmCirclePoint(&tc->coords.circle.xyz,
progress * tc->coords.circle.xyz.angle / tc->target,
&xyz);
// abc moves proportionally in order to end at the same time as the
// circular xyz move.
pmLinePoint(&tc->coords.circle.abc,
progress * tc->coords.circle.abc.tmag / tc->target,
&abc);
// same for uvw
pmLinePoint(&tc->coords.circle.uvw,
progress * tc->coords.circle.uvw.tmag / tc->target,
&uvw);
} else {
int s, tmp1,i;
double u,*N,R, X, Y, Z, A, B, C, U, V, W, F, D;
double curve_accel;
#if(TRACE != 0)
double delta_l, delta_u, delta_d, delta_x, delta_y, delta_z, delta_a;
#endif
N = tc->nurbs_block.N;
// NL = tc->nurbs_block.NL;
assert(tc->motion_type == TC_NURBS);
u = progress / tc->target;
if (u<1) {
s = nurbs_findspan(tc->nurbs_block.nr_of_ctrl_pts-1, tc->nurbs_block.order - 1,
u, tc->nurbs_block.knots_ptr); //return span index of u_i
nurbs_basisfun(s, u, tc->nurbs_block.order - 1 , tc->nurbs_block.knots_ptr , N); // input: s:knot span index u:u_0 d:B-Spline degree k:Knots
// output: N:basis functions
// refer to bspeval.cc::line(70) of octave
// refer to opennurbs_evaluate_nurbs.cpp::line(985) of openNurbs
// refer to ON_NurbsCurve::Evaluate() for ...
// refer to opennurbs_knot.cpp::ON_NurbsSpanIndex()
// http://www.rhino3d.com/nurbs.htm (What is NURBS?)
// Some modelers that use older algorithms for NURBS
// evaluation require two extra knot values for a total of
// degree+N+1 knots. When Rhino is exporting and importing
// NURBS geometry, it automatically adds and removes these
// two superfluous knots as the situation requires.
tmp1 = s - tc->nurbs_block.order + 1;
assert(tmp1 >= 0);
assert(tmp1 < tc->nurbs_block.nr_of_ctrl_pts);
R = 0.0;
for (i=0; i<=tc->nurbs_block.order -1 ; i++) {
R += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].R;
}
X = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
X += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].X;
}
X = X/R;
xyz.tran.x = X;
Y = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
Y += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].Y;
}
Y = Y/R;
xyz.tran.y = Y;
Z = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
Z += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].Z;
}
Z = Z/R;
xyz.tran.z = Z;
A = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
A += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].A;
}
A = A/R;
abc.tran.x = A;
B = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
B += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].B;
}
B = B/R;
abc.tran.y = B;
C = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
C += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].C;
}
C = C/R;
abc.tran.z = C;
U = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
U += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].U;
}
U = U/R;
uvw.tran.x = U;
V = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
V += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].V;
}
V = V/R;
uvw.tran.y = V;
W = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
W += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].W;
}
W = W/R;
uvw.tran.z = W;
F = 0.0;
F = tc->nurbs_block.ctrl_pts_ptr[tmp1].F;
tc->reqvel = F;
D = 0.0;
for (i=0; i<=tc->nurbs_block.order -1; i++) {
D += N[i]*tc->nurbs_block.ctrl_pts_ptr[tmp1+i].D;
}
D = D/R;
// compute allowed feed
if(!of_endpoint) {
curve_accel = (tc->cur_vel * tc->cur_vel)/D;
if(curve_accel > tc->maxaccel) {
// modify req_vel
tc->reqvel = pmSqrt((tc->maxaccel * D));
}
}
#if (TRACE != 0)
if(l == 0 && _dt == 0) {
last_l = 0;
last_u = 0;
last_x = xyz.tran.x;
last_y = xyz.tran.y;
last_z = xyz.tran.z;
last_a = 0;
_dt+=1;
}
delta_l = l - last_l;
last_l = l;
delta_u = u - last_u;
last_u = u;
delta_x = xyz.tran.x - last_x;
delta_y = xyz.tran.y - last_y;
delta_z = xyz.tran.z - last_z;
delta_a = abc.tran.x - last_a;
delta_d = pmSqrt(pmSq(delta_x)+pmSq(delta_y)+pmSq(delta_z));
last_x = xyz.tran.x;
last_y = xyz.tran.y;
last_z = xyz.tran.z;
last_a = abc.tran.x;
if( delta_d > 0)
{
if(_dt == 1){
/* prepare header for gnuplot */
DPS ("%11s%15s%15s%15s%15s%15s%15s%15s%15s\n",
"#dt", "u", "l","x","y","z","delta_d", "delta_l","a");
}
DPS("%11u%15.10f%15.10f%15.5f%15.5f%15.5f%15.5f%15.5f%15.5f\n",
_dt, u, l,last_x, last_y, last_z, delta_d, delta_l, last_a);
_dt+=1;
}
#endif // (TRACE != 0)
}else {
xyz.tran.x = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].X;
xyz.tran.y = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].Y;
xyz.tran.z = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].Z;
uvw.tran.x = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].U;
uvw.tran.y = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].V;
uvw.tran.z = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].W;
abc.tran.x = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].A;
abc.tran.y = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].B;
abc.tran.z = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].C;
// R = tc->nurbs_block.ctrl_pts_ptr[tc->nurbs_block.nr_of_ctrl_pts-1].R;
}
}
//DP ("GetEndPoint?(%d) R(%.2f) X(%.2f) Y(%.2f) Z(%.2f) A(%.2f)\n",of_endpoint, R, X, Y, Z, A);
// TODO-eric if R going to show ?
//#if (TRACE != 0)
// if(_dt == 0){
// /* prepare header for gnuplot */
// DPS ("%11s%15s%15s%15s\n", "#dt", "x", "y", "z");
// }
// DPS("%11u%15.5f%15.5f%15.5f\n", _dt, xyz.tran.x, xyz.tran.y, xyz.tran.z);
// _dt+=1;
//#endif // (TRACE != 0)
#if (TRACE != 1)
if( of_endpoint != 1) {
}
#endif
pos.tran = xyz.tran;
pos.a = abc.tran.x;
pos.b = abc.tran.y;
pos.c = abc.tran.z;
pos.u = uvw.tran.x;
pos.v = uvw.tran.y;
pos.w = uvw.tran.z;
// DP ("GetEndPoint?(%d) tc->id %d MotionType %d X(%.2f) Y(%.2f) Z(%.2f) A(%.2f)\n",
// of_endpoint,tc->id,tc->motion_type, pos.tran.x,
// pos.tran.y, pos.tran.z, pos.a);
return pos;
}
int pmQuatIsNorm(PmQuaternion const * const q1)
{
return (rtapi_fabs(pmSq(q1->s) + pmSq(q1->x) + pmSq(q1->y) + pmSq(q1->z) - 1.0) <
UNIT_QUAT_FUZZ);
}