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 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 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;
}
EmcPose tcGetPos(TC_STRUCT *tc)
{
EmcPose v;
PmPose v1;
PmPose v2;
if (0 == tc)
{
v1.tran.x = v1.tran.y = v1.tran.z = 0.0;
v1.rot.s = 1.0;
v1.rot.x = v1.rot.y = v1.rot.z = 0.0;
return v;
}
/* note: was if tc->targetPos <= 0.0 return basePos */
if (tc->type == TC_LINEAR)
{
pmLinePoint(&tc->line, tc->currentPos, &v1);
}
else if (tc->type == TC_CIRCULAR)
{
pmCirclePoint(&tc->circle, tc->currentPos / tc->circle.radius, &v1);
}
else
{
v1.tran.x = v1.tran.y = v1.tran.z = 0.0;
v1.rot.s = 1.0;
v1.rot.x = v1.rot.y = v1.rot.z = 0.0;
}
v.tran = v1.tran;
if(tc->abc_mag > 1e-6)
{
if(tc->tmag > 1e-6 )
{
pmLinePoint(&tc->line_abc,(tc->currentPos *tc->abc_mag /tc->tmag),&v2);
v.a = v2.tran.x;
v.b = v2.tran.y;
v.c = v2.tran.z;
}
else
{
pmLinePoint(&tc->line_abc,tc->currentPos,&v2);
v.a = v2.tran.x;
v.b = v2.tran.y;
v.c = v2.tran.z;
}
}
else
{
v.a = v.b = v.c = 0.0;
}
return v;
}
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 tcGetIntersectionPoint(TC_STRUCT const * const prev_tc,
TC_STRUCT const * const tc, PmCartesian * const point)
{
// TODO NULL pointer check?
// Get intersection point from geometry
if (tc->motion_type == TC_LINEAR) {
*point = tc->coords.line.xyz.start;
} else if (prev_tc->motion_type == TC_LINEAR) {
*point = prev_tc->coords.line.xyz.end;
} else if (tc->motion_type == TC_CIRCULAR){
pmCirclePoint(&tc->coords.circle.xyz, 0.0, point);
} else {
return TP_ERR_FAIL;
}
return TP_ERR_OK;
}
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;
}
/**
* 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;
}
EmcPose tcGetGoalPos(TC_STRUCT *tc)
{
PmPose v;
EmcPose ev;
if (0 == tc)
{
ev.tran.x = ev.tran.y = ev.tran.z = 0.0;
ev.a = ev.b = ev.c = 0.0;
return ev;
}
if (tc->type == TC_LINEAR)
{
v = tc->line.end;
}
else if (tc->type == TC_CIRCULAR)
{
/* we don't save start or end vector in TC_STRUCT to save space.
To get end, call pmCirclePoint with final angle. tcGetGoalPos()
is called infrequently so this space-time tradeoff is done. If
this function is called often, we should save end point in the
PM_CIRCLE struct. This will increase all TC_STRUCTS but make
tcGetGoalPos() run faster. */
pmCirclePoint(&tc->circle, tc->circle.angle, &v);
}
else
{
v.tran.x = v.tran.y = v.tran.z = 0.0;
v.rot.s = 1.0;
v.rot.x = v.rot.y = v.rot.z = 0.0;
}
ev.tran = v.tran;
ev.a = tc->line_abc.end.tran.x;
ev.b = tc->line_abc.end.tran.y;
ev.c = tc->line_abc.end.tran.z;
return ev;
}
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;
}
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 (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 { //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);
}
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;
return pos;
}
int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos)
{
PmCartesian xyz;
PmCartesian abc;
PmCartesian uvw;
double progress=0.0;
switch (of_point) {
case TC_GET_PROGRESS:
progress = tc->progress;
break;
case TC_GET_ENDPOINT:
progress = tc->target;
break;
case TC_GET_STARTPOINT:
progress = 0.0;
break;
}
switch (tc->motion_type){
case TC_RIGIDTAP:
if(tc->coords.rigidtap.state > REVERSING) {
pmCartLinePoint(&tc->coords.rigidtap.aux_xyz, progress, &xyz);
} else {
pmCartLinePoint(&tc->coords.rigidtap.xyz, progress, &xyz);
}
// no rotary move allowed while tapping
abc = tc->coords.rigidtap.abc;
uvw = tc->coords.rigidtap.uvw;
break;
case TC_LINEAR:
pmCartLinePoint(&tc->coords.line.xyz,
progress * tc->coords.line.xyz.tmag / tc->target,
&xyz);
pmCartLinePoint(&tc->coords.line.uvw,
progress * tc->coords.line.uvw.tmag / tc->target,
&uvw);
pmCartLinePoint(&tc->coords.line.abc,
progress * tc->coords.line.abc.tmag / tc->target,
&abc);
break;
case TC_CIRCULAR:
pmCirclePoint(&tc->coords.circle.xyz,
progress * tc->coords.circle.xyz.angle / tc->target,
&xyz);
pmCartLinePoint(&tc->coords.circle.abc,
progress * tc->coords.circle.abc.tmag / tc->target,
&abc);
pmCartLinePoint(&tc->coords.circle.uvw,
progress * tc->coords.circle.uvw.tmag / tc->target,
&uvw);
break;
case TC_SPHERICAL:
arcPoint(&tc->coords.arc.xyz,
progress,
&xyz);
abc = tc->coords.arc.abc;
uvw = tc->coords.arc.uvw;
break;
}
pmCartesianToEmcPose(&xyz, &abc, &uvw, pos);
return 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;
}
