Пример #1
0
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;
}
Пример #2
0
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;
}
Пример #3
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,&currentPose);
      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;
}
Пример #4
0
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;
}
Пример #5
0
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;
}
Пример #6
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;
}
Пример #7
0
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;
}
Пример #8
0
/**
 * 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;
}
Пример #9
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;
}
Пример #10
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;
}
Пример #11
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;
}
Пример #12
0
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;
}
Пример #13
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;
}