Working SLERP and spherical arcs

author: Robert W. Ellenberg <rwe24g@gmail.com> 2014-01-11 02:14:38 -0500
committer: Chris Radek <chris@timeguy.com> 2014-06-11 14:43:58 -0500
commit: 0eee499d9b4b018ced230e2007ac4f4f0c85e1c0 (patch)
tree: 4830cfae3c58c653c220a4af1f457d753d524dbf
parent: 0466ec51b3f73393b929658891d6cca6f0995520 (diff)
download: linuxcnc-0eee499d9b4b018ced230e2007ac4f4f0c85e1c0.tar.gz
linuxcnc-0eee499d9b4b018ced230e2007ac4f4f0c85e1c0.zip
4 files changed, 74 insertions, 13 deletions
diff --git a/src/emc/kinematics/spherical_arc.c b/src/emc/kinematics/spherical_arc.c
index d772c17ee..59bbd0e0d 100644
--- a/src/emc/kinematics/spherical_arc.c
+++ b/src/emc/kinematics/spherical_arc.c
@@ -14,6 +14,8 @@
 #include "posemath.h"
 #include "spherical_arc.h"
 #include "rtapi_math.h"
+#include "rtapi.h"
+#include "tp_debug.h"
 
 int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start,
         PmCartesian const * const end,
@@ -41,6 +43,7 @@ int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start,
     pmCartMag(&arc->rEnd, &mag1);
 
     if (fabs(mag0-mag1) > ARC_POS_EPSILON) {
+        tp_debug_print("radii %f and %f are different, aborting...\n", mag0, mag1);
         return ARC_ERR_GEOM;
     }
 
@@ -64,6 +67,7 @@ int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start,
     if (arc->angle < ARC_MIN_ANGLE) {
         return ARC_ERR_GEOM;
     }
+    arc->sinAngle = sin(arc->angle);
 
     return ARC_ERR_OK;
 }
@@ -71,14 +75,58 @@ int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start,
 int arcPoint(SphericalArc const * const arc, double angle_in, PmCartesian * const out)
 {
     //TODO pedantic
+    tp_debug_print("angle_in = %f, angle_total = %f\n",angle_in, arc->angle);
 
-    double scale0 = sin(arc->angle - angle_in) / sin(arc->angle);
-    double scale1 = sin(angle_in) / sin(arc->angle);
+    double scale0 = sin(arc->angle - angle_in) / arc->sinAngle;
+    double scale1 = sin(angle_in) / arc->sinAngle;
 
     PmCartesian interp0,interp1;
     pmCartScalMult(&arc->rStart, scale0, &interp0);
     pmCartScalMult(&arc->rEnd, scale1, &interp1);
 
     pmCartCartAdd(&interp0, &interp1, out);
+    pmCartCartAdd(&arc->center, out, out);
     return ARC_ERR_OK;
 }
+
+int arcLength(SphericalArc const * const arc, double * const length)
+{
+    *length = arc->radius * arc->angle;
+    return ARC_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) {
+
+    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);
+    pmCartUnit(&normal,&normal);
+    pmCartScalMult(&normal, center_dist, &normal);
+    pmCartCartAdd(middle, &normal, &center);
+
+    //Calculate the binormal (vector perpendicular to the plane of the
+    //arc)
+    pmCartCartCross(&line1->uVec, &line2->uVec, &binormal);
+    pmCartUnit(&binormal, &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);
+    pmCartCartAdd(end, middle, end);
+
+    return arcInitFromPoints(arc, start, end, &center);
+}
+
diff --git a/src/emc/kinematics/spherical_arc.h b/src/emc/kinematics/spherical_arc.h
index af3fdf383..5da4931b0 100644
--- a/src/emc/kinematics/spherical_arc.h
+++ b/src/emc/kinematics/spherical_arc.h
@@ -41,6 +41,7 @@ typedef struct {
     double radius;
     // Angle that the arc encloses
     double angle;
+    double sinAngle;
 } SphericalArc;
 
 typedef struct {
@@ -61,4 +62,8 @@ int arcPoint(SphericalArc const * const arc, double angle_in, PmCartesian * cons
 int arcNormalizedSlerp(SphericalArc const * const arc, double t, PmCartesian * const out);
 
 int arcLength(SphericalArc const * const arc, double * const length);
+
+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);
 #endif
diff --git a/src/emc/kinematics/tc.c b/src/emc/kinematics/tc.c
index 1988a3fa7..feff2b22e 100644
--- a/src/emc/kinematics/tc.c
+++ b/src/emc/kinematics/tc.c
@@ -260,15 +260,15 @@ int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos)
             break;
         case TC_SPHERICAL:
             arcPoint(&tc->coords.arc.xyz,
-                    progress * tc->coords.circle.xyz.angle / tc->target,
+                    progress * tc->coords.arc.xyz.angle / tc->target,
                     &xyz);
             // abc moves proportionally in order to end at the same time as the
             // circular xyz move.
-            pmCartLinePoint(&tc->coords.circle.abc,
+            pmCartLinePoint(&tc->coords.arc.abc,
                     progress * tc->coords.arc.abc.tmag / tc->target,
                     &abc);
             // same for uvw
-            pmCartLinePoint(&tc->coords.circle.uvw,
+            pmCartLinePoint(&tc->coords.arc.uvw,
                     progress * tc->coords.arc.uvw.tmag / tc->target,
                     &uvw);
             break;
diff --git a/src/emc/kinematics/tp.c b/src/emc/kinematics/tp.c
index 14bed523a..60f5abb58 100644
--- a/src/emc/kinematics/tp.c
+++ b/src/emc/kinematics/tp.c
@@ -138,6 +138,12 @@ STATIC int tpRotaryMotionCheck(TP_STRUCT const * const tp, TC_STRUCT const * con
             } else {
                 return true;
             }
+        case TC_SPHERICAL:
+            if (tc->coords.arc.abc.tmag_zero && tc->coords.arc.uvw.tmag_zero) {
+                return false;
+            } else {
+                return true;
+            }
         default:
             tp_debug_print("Unknown motion type!\n");
             return false;
@@ -277,7 +283,7 @@ STATIC inline double tpGetScaledAccel(TP_STRUCT const * const tp,
     if (tc->term_cond == TC_TERM_COND_PARABOLIC || tc->blend_prev) {
         a_scale *= 0.5;
     }
-    if (tc->motion_type == TC_CIRCULAR) {
+    if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) {
         //Limit acceleration for cirular arcs to allow for normal acceleration
         a_scale *= TP_ACC_RATIO_TANGENTIAL;
     }
@@ -719,7 +725,7 @@ STATIC int tpInitBlendArc(TP_STRUCT const * const tp, TC_STRUCT const * const pr
     // Treating arc as extension of prev_line_tc
     blend_tc->atspeed = prev_line_tc->atspeed;
 
-    blend_tc->motion_type = TC_CIRCULAR;
+    blend_tc->motion_type = TC_SPHERICAL;
 
 #ifdef TP_SHOW_BLENDS
     blend_tc->canon_motion_type = EMC_MOTION_TYPE_ARC;
@@ -734,13 +740,13 @@ STATIC int tpInitBlendArc(TP_STRUCT const * const tp, TC_STRUCT const * const pr
 
     blend_tc->syncdio = prev_line_tc->syncdio; //enqueue the list of DIOs that need toggling
 
-    double length;
     if (tpErrorCheck(tp)<0){
         return TP_ERR_FAIL;
     }
 
     // find "helix" length
-    length = blend_tc->coords.circle.xyz.angle * blend_tc->coords.circle.xyz.radius;
+    double length;
+    arcLength(&blend_tc->coords.arc.xyz, &length);
     blend_tc->target = length;
     //Blend arc specific settings:
     tcSetTermCond(blend_tc, TC_TERM_COND_TANGENT);
@@ -941,14 +947,16 @@ STATIC int tpCreateBlendArc(TP_STRUCT const * const tp, TC_STRUCT * const prev_t
     PmCartesian circ_start, circ_end;
 
     double h_plan = R_plan / Stheta;
-    pmCircleFromLines(&blend_tc->coords.circle.xyz,
+    arcFromLines(&blend_tc->coords.arc.xyz,
             &prev_tc->coords.line.xyz,
             &tc->coords.line.xyz, R_plan, d_plan, h_plan, &circ_start, &circ_end);
-    tp_debug_print("angle = %f\n",blend_tc->coords.circle.xyz.angle);
+    tp_debug_print("angle = %f\n",blend_tc->coords.arc.xyz.angle);
+
+    tp_debug_print("R_plan = %f, radius_calc = %f\n", R_plan, blend_tc->coords.arc.xyz.radius);
 
     // Note that previous restrictions don't allow ABC or UVW movement, so the end and start points should be identical
-    pmCartLineInit(&blend_tc->coords.circle.abc, &prev_tc->coords.line.abc.end, &tc->coords.line.abc.start);
-    pmCartLineInit(&blend_tc->coords.circle.uvw, &prev_tc->coords.line.uvw.end, &tc->coords.line.uvw.start);
+    pmCartLineInit(&blend_tc->coords.arc.abc, &prev_tc->coords.line.abc.end, &tc->coords.line.abc.start);
+    pmCartLineInit(&blend_tc->coords.arc.uvw, &prev_tc->coords.line.uvw.end, &tc->coords.line.uvw.start);
 
     //set the max velocity to v_plan, since we'll violate constraints otherwise.
     tpInitBlendArc(tp, prev_tc, blend_tc, v_actual, v_plan, a_max);
author	Robert W. Ellenberg <rwe24g@gmail.com>	2014-01-11 02:14:38 -0500
committer	Chris Radek <chris@timeguy.com>	2014-06-11 14:43:58 -0500
commit	0eee499d9b4b018ced230e2007ac4f4f0c85e1c0 (patch)
tree	4830cfae3c58c653c220a4af1f457d753d524dbf
parent	0466ec51b3f73393b929658891d6cca6f0995520 (diff)
download	linuxcnc-0eee499d9b4b018ced230e2007ac4f4f0c85e1c0.tar.gz linuxcnc-0eee499d9b4b018ced230e2007ac4f4f0c85e1c0.zip