grbl 中mc_arc()圓弧插補

#ifdef USE_LINE_NUMBERS
void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate, 
uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc, int32_t line_number)
#else
void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate,
uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc)
#endif
{

//第一步。確定圓心坐標，圓心->起始點的向量，圓心->終止點的向量
float center_axis0 = position[axis_0] + offset[axis_0];
float center_axis1 = position[axis_1] + offset[axis_1];
float r_axis0 = -offset[axis_0]; // Radius vector from center to current location
float r_axis1 = -offset[axis_1];
float rt_axis0 = target[axis_0] - center_axis0;
float rt_axis1 = target[axis_1] - center_axis1;

// CCW angle between position and target from circle center. Only one atan2() trig computation required.

//第二步。計算兩向量的夾角（弧度）atan2()是math.h中的函數，向量a(x1,y1),向量b(x2,y2),則他們的叉乘x1y2-x2y1;點積x1x2+y1y2

//atan2(叉乘，點積)可以求出兩向量夾角的弧度。
float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
if (is_clockwise_arc) { // Correct atan2 output per direction
if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= 2*M_PI; }
} else {
if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += 2*M_PI; }
}

// NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to
// (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit
// is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation.
// For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases.

//第三步計算圓弧可以分為多少條線段arc_tolerance小線段距離圓弧的最大距離。radius半徑小線段的一半是0.5*sqrt(r^2-(2r-a)^2)=0.5*sqrt(a(2r-a))
uint16_t segments = floor(fabs(0.5*angular_travel*radius)/
sqrt(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) );

if (segments) { 
// Multiply inverse feed_rate to compensate for the fact that this movement is approximated
// by a number of discrete segments. The inverse feed_rate should be correct for the sum of 
// all segments.
if (invert_feed_rate) { feed_rate *= segments; }

float theta_per_segment = angular_travel/segments;
float linear_per_segment = (target[axis_linear] - position[axis_linear])/segments;

/* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
and phi is the angle of rotation. Solution approach by Jens Geisler.
r_T = [cos(phi) -sin(phi);
sin(phi) cos(phi] * r ;

For arc generation, the center of the circle is the axis of rotation and the radius vector is 
defined from the circle center to the initial position. Each line segment is formed by successive
vector rotations. Single precision values can accumulate error greater than tool precision in rare
cases. So, exact arc path correction is implemented. This approach avoids the problem of too many very
expensive trig operations [sin(),cos(),tan()] which can take 100-200 usec each to compute.

Small angle approximation may be used to reduce computation overhead further. A third-order approximation
(second order sin() has too much error) holds for most, if not, all CNC applications. Note that this 
approximation will begin to accumulate a numerical drift error when theta_per_segment is greater than 
~0.25 rad(14 deg) AND the approximation is successively used without correction several dozen times. This
scenario is extremely unlikely, since segment lengths and theta_per_segment are automatically generated
and scaled by the arc tolerance setting. Only a very large arc tolerance setting, unrealistic for CNC 
applications, would cause this numerical drift error. However, it is best to set N_ARC_CORRECTION from a
low of ~4 to a high of ~20 or so to avoid trig operations while keeping arc generation accurate.

This approximation also allows mc_arc to immediately insert a line segment into the planner 
without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. 
This is important when there are successive arc motions. 
*/
// Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec

//第四步，求半徑和每條小線段的夾角T的sinT,cosT,這里用的近似算法https://wenku.baidu.com/view/34498178ba0d4a7303763a33.html

//這里用到三階，即節約運算時間又比較精確。
float cos_T = 2.0 - theta_per_segment*theta_per_segment;
float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0);
cos_T *= 0.5;

float sin_Ti;
float cos_Ti;
float r_axisi;
uint16_t i;
uint8_t count = 0;

for (i = 1; i<segments; i++) { // Increment (segments-1).

if (count < N_ARC_CORRECTION) {
// Apply vector rotation matrix. ~40 usec
r_axisi = r_axis0*sin_T + r_axis1*cos_T;
r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
r_axis1 = r_axisi;
count++;
} else { 
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. ~375 usec
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).

//第五步，用極坐標求每一條小線段的終點坐標，起始點與X軸的夾角為a，則終點x坐標rcos(a+T),y坐標rsin(a+T),cos(α+β)=cosαcosβ-sinαsinβ,sin(α+β)=sinαcosβ+cosαsinβ
cos_Ti = cos(i*theta_per_segment);
sin_Ti = sin(i*theta_per_segment);
r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
count = 0;
}

// Update arc_target location
position[axis_0] = center_axis0 + r_axis0;
position[axis_1] = center_axis1 + r_axis1;
position[axis_linear] += linear_per_segment;

#ifdef USE_LINE_NUMBERS
mc_line(position, feed_rate, invert_feed_rate, line_number);
#else
mc_line(position, feed_rate, invert_feed_rate);
#endif

// Bail mid-circle on system abort. Runtime command check already performed by mc_line.
if (sys.abort) { return; }
}
}
// Ensure last segment arrives at target location.
#ifdef USE_LINE_NUMBERS
mc_line(target, feed_rate, invert_feed_rate, line_number);
#else
mc_line(target, feed_rate, invert_feed_rate);
#endif
}