int main(){
double a1,a2,b1,b2,c1,c2,a,b,c,s;
while(1){
if(scanf("%lf%lf%lf%lf%lf%lf",&a1,&a2,&b1,&b2,&c1,&c2)!=6)return 0;
a=HYPOT(a1,a2,b1,b2);b=HYPOT(b1,b2,c1,c2);c=HYPOT(c1,c2,a1,a2);s=a+b+c;
printf("%.2lf\n",2*3.141592653589793*(a*b*c)/sqrt(s*(s-2*a)*(s-2*b)*(s-2*c)));
}
}
/* DO_ANGLE: Calculate angle (degrees) between vector 1 at (0,0; x1,y1) and
* vector 2 at (0,0; x2,y2).
*/
double do_angled(double x1, double y1, double x2, double y2)
{
double temp;
temp = ((x1 * x2) + (y1 * y2)) / (HYPOT(x1, y1) * HYPOT(x2, y2));
errno = 0;
temp = r2d(acos(temp));
MATH_ERROR("angled");
return (temp);
}
// ##########added by liu xiang ###########
//find the radiu of the image
void FindRadius1(unsigned char *Y[CAMNUM], double *CenX, double *CenY,int size)
{
int x, y;
double temp_radius;
unsigned char *pImage;
int i;
// Find maximum radius from center of mass
m_radius = 0;
for(i=0; i<size; i++)
{
pImage = Y[i];
for(y=0 ; y<HEIGHT; y++)
for(x=0 ; x<WIDTH ; x++)
{
if( *pImage < 255 )
{
// temp_radius = HYPOT(x - CENTER_X, y - CENTER_Y);
temp_radius = HYPOT(x - CenX[i], y - CenY[i]);
if(temp_radius > m_radius)
m_radius = temp_radius;
}
pImage++;
}
}
//ART_LUT_RADIUS =50
//这里要除以50是因为需要把他们归一化到半径为50的圆中
r_radius = ART_LUT_RADIUS / m_radius;
}
// speed up this function later
//作用即设定全局变量r_radius，即扫描图像中所有点，找到与中心点欧式距离最大值
//最后再用ART_LUT_RADIUS（50）除以最大值赋值
void FindRadius(unsigned char *Y[CAMNUM], double *CenX, double *CenY)
{
int x, y;
double temp_radius;
unsigned char *pImage;
int i;
// Find maximum radius from center of mass
m_radius = 0;
for(i=0; i<CAMNUM; i++)
{
pImage = Y[i];
for(y=0 ; y<HEIGHT; y++)
for(x=0 ; x<WIDTH ; x++)
{
// if( *pImage < 127 )
if( *pImage < 255 )
{
// temp_radius = HYPOT(x - CENTER_X, y - CENTER_Y);
temp_radius = HYPOT(x - CenX[i], y - CenY[i]);
if(temp_radius > m_radius)
m_radius = temp_radius;
}
pImage++;
}
}
r_radius = ART_LUT_RADIUS / m_radius;
}
void GenerateBasisLUT()
{
double angle, temp, radius;
int p, r, x, y;
int maxradius;
maxradius = ART_LUT_RADIUS;
for(y=0 ; y<ART_LUT_SIZE ; y++)
for(x=0 ; x<ART_LUT_SIZE ; x++)
{
radius = HYPOT(x-maxradius, y-maxradius);
if(radius < maxradius)
{
angle = atan2(y-maxradius, x-maxradius);
for(p=0 ; p<ART_ANGULAR ; p++)
for(r=0 ; r<ART_RADIAL ; r++)
{
temp = cos(radius*PI*r/maxradius);
m_pBasisR[p][r][x][y] = temp*cos(angle*p);
m_pBasisI[p][r][x][y] = temp*sin(angle*p);
}
}
else
{
for(p=0 ; p<ART_ANGULAR ; p++)
for(r=0 ; r<ART_RADIAL ; r++)
{
m_pBasisR[p][r][x][y] = 0;
m_pBasisI[p][r][x][y] = 0;
}
}
}
}
/**
* print_line_from_here_to_there() takes two cartesian coordinates and draws a line from one
* to the other. But there are really three sets of coordinates involved. The first coordinate
* is the present location of the nozzle. We don't necessarily want to print from this location.
* We first need to move the nozzle to the start of line segment where we want to print. Once
* there, we can use the two coordinates supplied to draw the line.
*
* Note: Although we assume the first set of coordinates is the start of the line and the second
* set of coordinates is the end of the line, it does not always work out that way. This function
* optimizes the movement to minimize the travel distance before it can start printing. This saves
* a lot of time and eliminates a lot of nonsensical movement of the nozzle. However, it does
* cause a lot of very little short retracement of th nozzle when it draws the very first line
* segment of a 'circle'. The time this requires is very short and is easily saved by the other
* cases where the optimization comes into play.
*/
void print_line_from_here_to_there(const float &sx, const float &sy, const float &sz, const float &ex, const float &ey, const float &ez) {
const float dx_s = current_position[X_AXIS] - sx, // find our distance from the start of the actual line segment
dy_s = current_position[Y_AXIS] - sy,
dist_start = HYPOT2(dx_s, dy_s), // We don't need to do a sqrt(), we can compare the distance^2
// to save computation time
dx_e = current_position[X_AXIS] - ex, // find our distance from the end of the actual line segment
dy_e = current_position[Y_AXIS] - ey,
dist_end = HYPOT2(dx_e, dy_e),
line_length = HYPOT(ex - sx, ey - sy);
// If the end point of the line is closer to the nozzle, flip the direction,
// moving from the end to the start. On very small lines the optimization isn't worth it.
if (dist_end < dist_start && (INTERSECTION_CIRCLE_RADIUS) < ABS(line_length))
return print_line_from_here_to_there(ex, ey, ez, sx, sy, sz);
// Decide whether to retract & bump
if (dist_start > 2.0) {
retract_filament(destination);
//todo: parameterize the bump height with a define
move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + 0.500, 0.0); // Z bump to minimize scraping
move_to(sx, sy, sz + 0.500, 0.0); // Get to the starting point with no extrusion while bumped
}
move_to(sx, sy, sz, 0.0); // Get to the starting point with no extrusion / un-Z bump
const float e_pos_delta = line_length * g26_e_axis_feedrate * g26_extrusion_multiplier;
recover_filament(destination);
move_to(ex, ey, ez, e_pos_delta); // Get to the ending point with an appropriate amount of extrusion
}
void debug_current_and_destination(PGM_P title) {
// if the title message starts with a '!' it is so important, we are going to
// ignore the status of the g26_debug_flag
if (*title != '!' && !g26_debug_flag) return;
const float de = destination[E_AXIS] - current_position[E_AXIS];
if (de == 0.0) return; // Printing moves only
const float dx = destination[X_AXIS] - current_position[X_AXIS],
dy = destination[Y_AXIS] - current_position[Y_AXIS],
xy_dist = HYPOT(dx, dy);
if (xy_dist == 0.0) return;
const float fpmm = de / xy_dist;
SERIAL_ECHOPAIR_F(" fpmm=", fpmm, 6);
SERIAL_ECHOPAIR_F(" current=( ", current_position[X_AXIS], 6);
SERIAL_ECHOPAIR_F(", ", current_position[Y_AXIS], 6);
SERIAL_ECHOPAIR_F(", ", current_position[Z_AXIS], 6);
SERIAL_ECHOPAIR_F(", ", current_position[E_AXIS], 6);
SERIAL_ECHOPGM(" ) destination=( "); debug_echo_axis(X_AXIS);
SERIAL_ECHOPGM(", "); debug_echo_axis(Y_AXIS);
SERIAL_ECHOPGM(", "); debug_echo_axis(Z_AXIS);
SERIAL_ECHOPGM(", "); debug_echo_axis(E_AXIS);
SERIAL_ECHOPGM(" ) ");
serialprintPGM(title);
SERIAL_EOL();
}
void FindRadius2(unsigned char *Y, double *CenX, double *CenY)
{
int x, y;
double temp_radius;
unsigned char *pImage;
int i;
// Find maximum radius from center of mass
m_radius = 0;
pImage = Y;
for(y=0 ; y<HEIGHT; y++)
for(x=0 ; x<WIDTH ; x++)
{
if( *pImage < 255 )
{
// temp_radius = HYPOT(x - CENTER_X, y - CENTER_Y);
temp_radius = HYPOT(x - *CenX, y - *CenY);
if(temp_radius > m_radius)
m_radius = temp_radius;
}
pImage++;
}
//ART_LUT_RADIUS =50
//这里要除以50是因为需要把他们归一化到半径为50的圆中
r_radius = ART_LUT_RADIUS / m_radius;
}
/* DO_DIST: Calculate distance between point 1 at (x1,y1) and
* point 2 at (x2,y2).
*/
double do_dist(double x1, double y1, double x2, double y2)
{
double temp;
errno = 0;
temp = HYPOT((x1 - x2), (y1 - y2));
MATH_ERROR("hypot");
return (temp);
}
double *point_distance(Point *pt1, Point *pt2)
{
double *result;
result = PALLOCTYPE(double);
*result = HYPOT( pt1->x - pt2->x, pt1->y - pt2->y );
return(result);
}
double *dist_pl(Point *pt, LINE *line)
{
double *result;
result = PALLOCTYPE(double);
*result = (line->A * pt->x + line->B * pt->y + line->C) /
HYPOT(line->A, line->B);
return(result);
}
static void compute_block(CSOUND *csound, PAULSTRETCH *p)
{
uint32_t istart_pos = floor(p->start_pos);
uint32_t pos;
uint32_t i;
uint32_t windowsize = p->windowsize;
uint32_t half_windowsize = p->half_windowsize;
MYFLT *hinv_buf = p->hinv_buf;
MYFLT *old_windowed_buf= p->old_windowed_buf;
MYFLT *tbl = p->ft->ftable;
MYFLT *window = p->window;
MYFLT *output= p->output;
MYFLT *tmp = p->tmp;
for (i = 0; i < windowsize; i++) {
pos = istart_pos + i;
if (pos < p->ft->flen) {
tmp[i] = tbl[pos] * window[i];
} else {
tmp[i] = 0;
}
}
/* re-order bins and take FFT */
tmp[p->windowsize] = tmp[1];
tmp[p->windowsize + 1] = 0.0;
csoundRealFFTnp2(csound, tmp, p->windowsize);
/* randomize phase */
for (i = 0; i < windowsize + 2; i += 2) {
MYFLT mag = HYPOT(tmp[i], tmp[i + 1]);
// Android 5.1 does not seem to have cexpf ...
// complex ph = cexpf(I * ((MYFLT)rand() / RAND_MAX) * 2 * M_PI);
// so ...
MYFLT x = (((MYFLT)rand() / RAND_MAX) * 2 * M_PI);
complex ph = cos(x) + I*sin(x);
tmp[i] = mag * (MYFLT)crealf(ph);
tmp[i + 1] = mag * (MYFLT)cimagf(ph);
}
/* re-order bins and take inverse FFT */
tmp[1] = tmp[p->windowsize];
csoundInverseRealFFTnp2(csound, tmp, p->windowsize);
/* apply window and overlap */
for (i = 0; i < windowsize; i++) {
tmp[i] *= window[i];
if (i < half_windowsize) {
output[i] = (MYFLT)(tmp[i] + old_windowed_buf[half_windowsize + i]);
output[i] *= hinv_buf[i];
}
old_windowed_buf[i] = tmp[i];
}
p->start_pos += p->displace_pos;
}
/* box_dt - returns the distance between the
* center points of two boxes.
*/
double box_dt(BOX *box1, BOX *box2)
{
double result;
Point *box_center(),
*a, *b;
a = box_center(box1);
b = box_center(box2);
result = HYPOT(a->x - b->x, a->y - b->y);
PFREE(a);
PFREE(b);
return(result);
}
/* box_distance - returns the distance between the
* center points of two boxes.
*/
double *box_distance(BOX *box1, BOX *box2)
{
double *result;
Point *box_center(), *a, *b;
result = PALLOCTYPE(double);
a = box_center(box1);
b = box_center(box2);
*result = HYPOT(a->x - b->x, a->y - b->y);
PFREE(a);
PFREE(b);
return(result);
}
mesh_index_pair find_closest_circle_to_print(const float &X, const float &Y) {
float closest = 99999.99;
mesh_index_pair return_val;
return_val.x_index = return_val.y_index = -1;
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
if (!is_bitmap_set(circle_flags, i, j)) {
const float mx = _GET_MESH_X(i), // We found a circle that needs to be printed
my = _GET_MESH_Y(j);
// Get the distance to this intersection
float f = HYPOT(X - mx, Y - my);
// It is possible that we are being called with the values
// to let us find the closest circle to the start position.
// But if this is not the case, add a small weighting to the
// distance calculation to help it choose a better place to continue.
f += HYPOT(g26_x_pos - mx, g26_y_pos - my) / 15.0;
// Add in the specified amount of Random Noise to our search
if (random_deviation > 1.0)
f += random(0.0, random_deviation);
if (f < closest) {
closest = f; // We found a closer location that is still
return_val.x_index = i; // un-printed --- save the data for it
return_val.y_index = j;
return_val.distance = closest;
}
}
}
}
bitmap_set(circle_flags, return_val.x_index, return_val.y_index); // Mark this location as done.
return return_val;
}
void GenerateBasisLUT()
{
double angle, temp, radius;
int p, r, x, y;
int maxradius;
maxradius = ART_LUT_RADIUS;//50
//ART_LUT_SIZE =101，为什么是101，是因为要包括100的距离，所以稍微大一点
//因为将所有的像素点集中到了2*50 * 2*50范围内，所以x，y的范围为0-100
for(y=0 ; y<ART_LUT_SIZE ; y++)
for(x=0 ; x<ART_LUT_SIZE ; x++)
{
//计算半径
radius = HYPOT((float)(x-maxradius), (float)(y-maxradius));
if(radius < maxradius)
{
//atan2：返回第一个参数除以第二个参数的反正切值（-Pi/2,Pi/2）
//计算改点与圆心之间的夹角
angle = atan2((float)(y-maxradius), (float)(x-maxradius));
//ART_ANGULAR =12
//ART_RADIAL = 3
//中间m_pBasisR[p][r][x][y]中针对不同的x，y；只要pr相同，其值不同，因为r什么的都变了
for(p=0 ; p<ART_ANGULAR ; p++)
for(r=0 ; r<ART_RADIAL ; r++)
{
//cos接受的参数自能是几π
temp = cos(radius*PI*r/maxradius);
m_pBasisR[p][r][x][y] = temp*cos(angle*p);
m_pBasisI[p][r][x][y] = temp*sin(angle*p);
}
}
else
{
for(p=0 ; p<ART_ANGULAR ; p++)
for(r=0 ; r<ART_RADIAL ; r++)
{
m_pBasisR[p][r][x][y] = 0;
m_pBasisI[p][r][x][y] = 0;
}
}
}
}
/**
* print_line_from_here_to_there() takes two cartesian coordinates and draws a line from one
* to the other. But there are really three sets of coordinates involved. The first coordinate
* is the present location of the nozzle. We don't necessarily want to print from this location.
* We first need to move the nozzle to the start of line segment where we want to print. Once
* there, we can use the two coordinates supplied to draw the line.
*
* Note: Although we assume the first set of coordinates is the start of the line and the second
* set of coordinates is the end of the line, it does not always work out that way. This function
* optimizes the movement to minimize the travel distance before it can start printing. This saves
* a lot of time and eleminates a lot of non-sensical movement of the nozzle. However, it does
* cause a lot of very little short retracement of th nozzle when it draws the very first line
* segment of a 'circle'. The time this requires is very short and is easily saved by the other
* cases where the optimization comes into play.
*/
void print_line_from_here_to_there(const float &sx, const float &sy, const float &sz, const float &ex, const float &ey, const float &ez) {
const float dx_s = current_position[X_AXIS] - sx, // find our distance from the start of the actual line segment
dy_s = current_position[Y_AXIS] - sy,
dist_start = HYPOT2(dx_s, dy_s), // We don't need to do a sqrt(), we can compare the distance^2
// to save computation time
dx_e = current_position[X_AXIS] - ex, // find our distance from the end of the actual line segment
dy_e = current_position[Y_AXIS] - ey,
dist_end = HYPOT2(dx_e, dy_e),
line_length = HYPOT(ex - sx, ey - sy);
// If the end point of the line is closer to the nozzle, flip the direction,
// moving from the end to the start. On very small lines the optimization isn't worth it.
if (dist_end < dist_start && (SIZE_OF_INTERSECTION_CIRCLES) < abs(line_length)) {
//if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" Reversing start and end of print_line_from_here_to_there()");
return print_line_from_here_to_there(ex, ey, ez, sx, sy, sz);
}
// Decide whether to retract.
if (dist_start > 2.0) {
retract_filament(destination);
//if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" filament retracted.");
}
move_to(sx, sy, sz, 0.0); // Get to the starting point with no extrusion
const float e_pos_delta = line_length * g26_e_axis_feedrate * extrusion_multiplier;
un_retract_filament(destination);
//if (ubl.g26_debug_flag) {
// SERIAL_ECHOLNPGM(" doing printing move.");
// debug_current_and_destination(PSTR("doing final move_to() inside print_line_from_here_to_there()"));
//}
move_to(ex, ey, ez, e_pos_delta); // Get to the ending point with an appropriate amount of extrusion
}
double point_dt(Point *pt1, Point *pt2)
{
return( HYPOT( pt1->x - pt2->x, pt1->y - pt2->y ) );
}
//Y表示照片的指针
void ExtractCoefficients(unsigned char *Y, double m_Coeff[ART_ANGULAR][ART_RADIAL], unsigned char *Edge, double CenX, double CenY)
{
int x, y, ix, iy;
int p, r;
double dx, dy, tx, ty, x1, x2;
int count;
double m_pCoeffR[ART_ANGULAR][ART_RADIAL];
double m_pCoeffI[ART_ANGULAR][ART_RADIAL];
// double norm;
unsigned char *pImage;
// unsigned char *pEdge;
memset(m_pCoeffR, 0, ART_ANGULAR * ART_RADIAL * sizeof(double) );
memset(m_pCoeffI, 0, ART_ANGULAR * ART_RADIAL * sizeof(double) );
// for(p=0 ; p<ART_ANGULAR ; p++)
// for(r=0 ; r<ART_RADIAL ; r++)
// {
// m_pCoeffR[p][r] = 0;
// m_pCoeffI[p][r] = 0;
// }
count = 0;
pImage = Y;
// pEdge = Edge;
for (y=0 ; y<HEIGHT ; y++)
for (x=0 ; x<WIDTH; x++)
{
// if( *pImage < 127 )
if( *pImage < 255 )
{
// 1.0 is for silhouette, another one is for depth, both weighting is 0.5
// both depth and silhouette
// norm = (1.0 + (255.0-*pImage) / 255.0) / 2;
// depth only
// norm = (255.0-*pImage) / 255.0;
// silhouette only
// norm = 1.0;
// edge (from depth) only
// norm = *pEdge / 255.0;
// edge (from depth) + depth + silhouette
// norm = (*pEdge/255.0 + (255.0-*pImage)/255.0 + 1.0) / 3;
// edge (from silhouette) only
// norm = 1.0;
// edge (from silhouette) + silhouette
// norm = (255.0-*pImage) / 255.0;
// map image coordinate (x,y) to basis function coordinate (tx,ty)
// dx = x - CENTER_X;
// dy = y - CENTER_Y;
dx = x - CenX;
dy = y - CenY;
tx = dx * r_radius + ART_LUT_RADIUS;//r_radius由上一个函数计算
ty = dy * r_radius + ART_LUT_RADIUS;//为什么要加上50的ART_LUT_RADIUS，因为dx是负的
ix = (int)tx;
iy = (int)ty;
dx = tx - ix;//dx表示小数部分，为啥要小数，用来干嘛？
dy = ty - iy;
//这里即通过分割地方法来近似积分
// summation of basis function
// if(tx >= 0 && tx < ART_LUT_SIZE && ty >= 0 && ty < ART_LUT_SIZE)
for(p=0 ; p<ART_ANGULAR ; p++)
for(r=0 ; r<ART_RADIAL ; r++)
{
// GetReal (if call function, the speed will be very slow)
// m_pCoeffR[p][r] += GetReal(p, r, tx, ty);
x1 = m_pBasisR[p][r][ix][iy] + (m_pBasisR[p][r][ix+1][iy]-m_pBasisR[p][r][ix][iy]) * dx;
x2 = m_pBasisR[p][r][ix][iy+1] + (m_pBasisR[p][r][ix+1][iy+1]-m_pBasisR[p][r][ix][iy+1]) * dx;
// m_pCoeffR[p][r] += norm * (x1 + (x2-x1) * dy);
m_pCoeffR[p][r] += (x1 + (x2-x1) * dy);
// GetImg (if call function, the speed will be very slow)
// m_pCoeffI[p][r] -= GetImg(p, r, tx, ty);
x1 = m_pBasisI[p][r][ix][iy] + (m_pBasisI[p][r][ix+1][iy]-m_pBasisI[p][r][ix][iy]) * dx;
x2 = m_pBasisI[p][r][ix][iy+1] + (m_pBasisI[p][r][ix+1][iy+1]-m_pBasisI[p][r][ix][iy+1]) * dx;
// m_pCoeffI[p][r] -= norm * (x1 + (x2-x1) * dy);
m_pCoeffI[p][r] -= (x1 + (x2-x1) * dy);
}
count ++; // how many pixels
}
pImage++;
// pEdge++;
}
// if the 3D model is flat, some camera will render nothing, so count=0 in this case
if( count > 0 )
{
for(p=0 ; p<ART_ANGULAR ; p++)
for(r=0 ; r<ART_RADIAL ; r++)
m_Coeff[p][r] = HYPOT( m_pCoeffR[p][r]/count, m_pCoeffI[p][r]/count );
// normalization
for(p=0 ; p<ART_ANGULAR ; p++)
for(r=0 ; r<ART_RADIAL ; r++)
m_Coeff[p][r] /= m_Coeff[0][0];
}
else
{
// if didn't add this, the result will also be saved as 0
for(p=0 ; p<ART_ANGULAR ; p++)
for(r=0 ; r<ART_RADIAL ; r++)
m_Coeff[p][r] = 0.0;
// use a line to test the number to approximate
/* for(p=0 ; p<ART_ANGULAR ; p++)
for(r=0 ; r<ART_RADIAL ; r++)
m_Coeff[p][r] = 0.010256410;
for(p=0 ; p<ART_ANGULAR ; p+=2)
m_Coeff[p][0] = 0.980129780;*/
}
}
bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate) {
if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate
const float total[XYZE] = {
rtarget[X_AXIS] - current_position[X_AXIS],
rtarget[Y_AXIS] - current_position[Y_AXIS],
rtarget[Z_AXIS] - current_position[Z_AXIS],
rtarget[E_AXIS] - current_position[E_AXIS]
};
const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]); // total horizontal xy distance
#if IS_KINEMATIC
const float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate
uint16_t segments = lroundf(delta_segments_per_second * seconds), // preferred number of segments for distance @ feedrate
seglimit = lroundf(cartesian_xy_mm * (1.0f / (DELTA_SEGMENT_MIN_LENGTH))); // number of segments at minimum segment length
NOMORE(segments, seglimit); // limit to minimum segment length (fewer segments)
#else
uint16_t segments = lroundf(cartesian_xy_mm * (1.0f / (DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length
#endif
NOLESS(segments, 1U); // must have at least one segment
const float inv_segments = 1.0f / segments; // divide once, multiply thereafter
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
scara_oldA = planner.get_axis_position_degrees(A_AXIS);
scara_oldB = planner.get_axis_position_degrees(B_AXIS);
#endif
const float diff[XYZE] = {
total[X_AXIS] * inv_segments,
total[Y_AXIS] * inv_segments,
total[Z_AXIS] * inv_segments,
total[E_AXIS] * inv_segments
};
// Note that E segment distance could vary slightly as z mesh height
// changes for each segment, but small enough to ignore.
float raw[XYZE] = {
current_position[X_AXIS],
current_position[Y_AXIS],
current_position[Z_AXIS],
current_position[E_AXIS]
};
// Only compute leveling per segment if ubl active and target below z_fade_height.
if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
while (--segments) {
LOOP_XYZE(i) raw[i] += diff[i];
ubl_buffer_segment_raw(raw, feedrate);
}
ubl_buffer_segment_raw(rtarget, feedrate);
return false; // moved but did not set_current_from_destination();
}
// Otherwise perform per-segment leveling
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]);
#endif
// increment to first segment destination
LOOP_XYZE(i) raw[i] += diff[i];
for (;;) { // for each mesh cell encountered during the move
// Compute mesh cell invariants that remain constant for all segments within cell.
// Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter)
// the bilinear interpolation from the adjacent cell within the mesh will still work.
// Inner loop will exit each time (because out of cell bounds) but will come back
// in top of loop and again re-find same adjacent cell and use it, just less efficient
// for mesh inset area.
int8_t cell_xi = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0f / (MESH_X_DIST)),
cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0f / (MESH_Y_DIST));
cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add
y0 = mesh_index_to_ypos(cell_yi);
float z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner
z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner
z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner
z_x1y1 = z_values[cell_xi+1][cell_yi+1]; // z at upper right corner
if (isnan(z_x0y0)) z_x0y0 = 0; // ideally activating planner.leveling_active (G29 A)
if (isnan(z_x1y0)) z_x1y0 = 0; // should refuse if any invalid mesh points
if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell,
if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points
float cx = raw[X_AXIS] - x0, // cell-relative x and y
cy = raw[Y_AXIS] - y0;
const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0f / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower right)
z_xmy1 = (z_x1y1 - z_x0y1) * (1.0f / (MESH_X_DIST)); // z slope per x along y1 (upper left to upper right)
float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx (changes for each cx in cell)
const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx
z_cxyd = z_cxy1 - z_cxy0; // z height difference along cx from y0 to y1
float z_cxym = z_cxyd * (1.0f / (MESH_Y_DIST)); // z slope per y along cx from y0 to y1 (changes for each cx in cell)
// float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy (do inside the segment loop)
// As subsequent segments step through this cell, the z_cxy0 intercept will change
// and the z_cxym slope will change, both as a function of cx within the cell, and
// each change by a constant for fixed segment lengths.
const float z_sxy0 = z_xmy0 * diff[X_AXIS], // per-segment adjustment to z_cxy0
z_sxym = (z_xmy1 - z_xmy0) * (1.0f / (MESH_Y_DIST)) * diff[X_AXIS]; // per-segment adjustment to z_cxym
for (;;) { // for all segments within this mesh cell
if (--segments == 0) // if this is last segment, use rtarget for exact
COPY(raw, rtarget);
const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
* fade_scaling_factor // apply fade factor to interpolated mesh height
#endif
;
const float z = raw[Z_AXIS];
raw[Z_AXIS] += z_cxcy;
ubl_buffer_segment_raw(raw, feedrate);
raw[Z_AXIS] = z;
if (segments == 0) // done with last segment
return false; // did not set_current_from_destination()
LOOP_XYZE(i) raw[i] += diff[i];
cx += diff[X_AXIS];
cy += diff[Y_AXIS];
if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) // done within this cell, break to next
break;
// Next segment still within same mesh cell, adjust the per-segment
// slope and intercept to compute next z height.
z_cxy0 += z_sxy0; // adjust z_cxy0 by per-segment z_sxy0
z_cxym += z_sxym; // adjust z_cxym by per-segment z_sxym
} // segment loop
} // cell loop
return false; // caller will update current_position
}
