( CUT HERE AND INCLUDE THE LINES BELOW TO USE ARCTO AND SPLINETO IN YOUR )
( OWN G-CODE FILES                                                       )

( Copyright 2007 Jeff Epler                                              )
( License: GPL V2                                                        )

( Public routines:                                                       )
( O301 call [x1] [y1] [dx] [dy]  -- arcfrom                              )
(   Set the initial conditions for arcto and splineto.  The X and Y      )
(   values must match the current location, and the DX and DY values     )
(   specify the initial direction for arcto                              )
(                                                                        )
( O302 call [x2] [y2] -- arcto                                           )
(   Go to x2 y2 with a line or arc that is tangent to the last location  )
(   as specified by a previous arcfrom, arcto, or splineto               )
(                                                                        )
( O310 call [cx1] [cy1] [cx2] [cy2] [x3] [y3] -- splineto                )
(   Approximate a spline from the last location to X3,Y3 with control    )
(   points CX1,CY1 and CX2,CY2.  The last location specified by arcfrom, )
(   arcto, or splineto is used, but the initial tangent is determined    )
(   from the spline's control points rather than the last tangent        )
(                                                                        )
( Public variables:                                                      )
( #305: Number of points for splineto interpolation.  Default: 16        )
(                                                                        )
( Private O-numbers and variables used:                                  )
( O301 O302 O303 O304 O305 O311                                          )
( #301 #302 #303 #304                                                    )

(convert #301 #302 to unit vector)
O302 sub (unit: )
    #1=[sqrt[#303*#303+#304*#304]]
    O303 if [#1 LE .00001]
        #303=0 #304=0
    O303 else
        #303=[#303/#1] #304=[#304/#1]
    O303 endif
O302 endsub

(set initial condition for arcto)
O301 sub (arcfrom: x1 y1 dx dy)
    #301=#1
    #302=#2
    #303=#3
    #304=#4
    O302 call
O301 endsub

(curve to x2 y2 from old position)
O300 sub (arcto: x2 y2)
    #3=[#1-#301]                     (x = x2-x1)
    #4=[#2-#302]                     (y = y2-y1)
    #5=[2*[#4*#303 - #3*#304]]       (den = ...)
    (print,1=#1,2=#2,3=#3,4=#4,5=#5)
    (print,103=#303,104=#304)
    O304 if [[#5 LE .00001] AND [#5 GE -.00001]]
        (print,line)
        G1 X#1 Y#2
        #303=[#1-#301]
        #304=[#2-#302]
    O304 else
        #6=[-1*[#3*#3 + #4*#4] / #5] (r = ...)
        #7=[#304*#6]                 (i = ...)
        #8=[-1*#303*#6]              (j = ...)
        #9=[#301+#7]                 (cx = ...)
        #10=[#302+#8]                (cy = ...)
        O305 if [#6 LT 0]             (counterclockwise)
            (print,counterclockwise)
            (print,G3 I#7 J#8 X#1 Y#2)
            G3 I#7 J#8 X#1 Y#2
            #303=[#10-#2] #304=[#1-#9]
        O305 else                    (clockwise)
            (print,clockwise)
            (print,G2 I#7 J#8 X#1 Y#2)
            G2 I#7 J#8 X#1 Y#2
            #303=[#2-#10] #304=[#9-#1]
        O305 endif
    O304 endif
    #301=#1
    #302=#2
    O302 call                 (unit)
O300 endsub

#305=16  (105: number of points used in spline->arc interpolation)
O310 sub (splineto: cx1 cy1 cx2 cy2 x3 y3)
#7=0
#17=#301
#18=#302
#303=[#1-#301]             (set the initial direction vector)
#304=[#2-#302]
O302 call                  (unit)
O311 while [#7 LT #305]
    #7=[#7+1]
    #8=[#7/#305]           (tf)
    #9=[1-#8]              (1-tf)
    #11=[#9*#9]            (SQ 1-tf )
    #12=[#11*#9]           (CUBE 1-tf)
    #13=[#8*#8]            (SQ tf)
    #14=[#13*#8]           (CUBE tf)
    #15=[#12*#17 + #11*3*#8*#1 + #13*3*#9*#3 + #14*#5]
    #16=[#12*#18 + #11*3*#8*#2 + #13*3*#9*#4 + #14*#6]
    (print,8=#8, x#15 y#16)
    O300 call [#15] [#16]  (arcto)
O311 endwhile
O310 endsub
( CUT HERE AND INCLUDE THE LINES ABOVE TO USE ARCTO AND SPLINETO IN YOUR )
( OWN G-CODE FILES                                                       )


( Demo of 'arcto': Starting at X0Y0 make a closed circuit passing        )
( through the given points                                               )
G20
G0 X0 Y0 Z1
G1 F100 Z-.1
O301 call [0] [0] [0] [0]
O300 call [2] [0]
O300 call [4] [1]
O300 call [4] [2]
O300 call [2] [3]
O300 call [0] [3]
O300 call [0] [0]
G0 Z1

( Demo of 'splineto': approximate the spline with start point X-1Y4, end )
( point X4Y4, and control points X1Y7 and X3Y1 with 4, 8, 16, and 32     )
( points.  4 points is not very good, 8 points is pretty good, and 16    )
( and 32 points are nearly indistinguishable.                            )

#305=4
G0 X-1 Y4
G1 F100 Z-.1
O301 call [-1] [4]
O310 call [1] [7] [3] [1] [4] [4]
G0 Z1

#305=8
G0 X-1 Y4
G1 F100 Z-.2
O301 call [-1] [4]
O310 call [1] [7] [3] [1] [4] [4]
G0 Z1

#305=16
G0 X-1 Y4
G1 F100 Z-.3
O301 call [-1] [4]
O310 call [1] [7] [3] [1] [4] [4]
G0 Z1

#305=32
G0 X-1 Y4
G1 F100 Z-.4
O301 call [-1] [4]
O310 call [1] [7] [3] [1] [4] [4]
G0 Z1

M2