|
|
|
@ -154,7 +154,7 @@ |
|
|
|
* to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide. |
|
|
|
*/ |
|
|
|
|
|
|
|
const float xratio = (RAW_X_POSITION(end[X_AXIS]) - pgm_read_float(&(ubl.mesh_index_to_xpos[cell_dest_xi]))) * (1.0 / (MESH_X_DIST)), |
|
|
|
const float xratio = (RAW_X_POSITION(end[X_AXIS]) - pgm_read_float(&ubl.mesh_index_to_xpos[cell_dest_xi])) * (1.0 / (MESH_X_DIST)), |
|
|
|
z1 = ubl.z_values[cell_dest_xi ][cell_dest_yi ] + xratio * |
|
|
|
(ubl.z_values[cell_dest_xi + 1][cell_dest_yi ] - ubl.z_values[cell_dest_xi][cell_dest_yi ]), |
|
|
|
z2 = ubl.z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio * |
|
|
|
@ -163,7 +163,7 @@ |
|
|
|
// we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
|
|
|
|
// are going to apply the Y-Distance into the cell to interpolate the final Z correction.
|
|
|
|
|
|
|
|
const float yratio = (RAW_Y_POSITION(end[Y_AXIS]) - pgm_read_float(&(ubl.mesh_index_to_ypos[cell_dest_yi]))) * (1.0 / (MESH_Y_DIST)); |
|
|
|
const float yratio = (RAW_Y_POSITION(end[Y_AXIS]) - pgm_read_float(&ubl.mesh_index_to_ypos[cell_dest_yi])) * (1.0 / (MESH_Y_DIST)); |
|
|
|
|
|
|
|
float z0 = z1 + (z2 - z1) * yratio; |
|
|
|
|
|
|
|
@ -198,8 +198,8 @@ |
|
|
|
const float dx = end[X_AXIS] - start[X_AXIS], |
|
|
|
dy = end[Y_AXIS] - start[Y_AXIS]; |
|
|
|
|
|
|
|
const int left_flag = dx < 0.0 ? 1.0 : 0.0, |
|
|
|
down_flag = dy < 0.0 ? 1.0 : 0.0; |
|
|
|
const int left_flag = dx < 0.0 ? 1 : 0, |
|
|
|
down_flag = dy < 0.0 ? 1 : 0; |
|
|
|
|
|
|
|
const float adx = left_flag ? -dx : dx, |
|
|
|
ady = down_flag ? -dy : dy; |
|
|
|
@ -230,8 +230,8 @@ |
|
|
|
const float m = dy / dx, |
|
|
|
c = start[Y_AXIS] - m * start[X_AXIS]; |
|
|
|
|
|
|
|
const bool inf_normalized_flag=isinf(e_normalized_dist), |
|
|
|
inf_m_flag=isinf(m); |
|
|
|
const bool inf_normalized_flag = isinf(e_normalized_dist), |
|
|
|
inf_m_flag = isinf(m); |
|
|
|
/**
|
|
|
|
* This block handles vertical lines. These are lines that stay within the same |
|
|
|
* X Cell column. They do not need to be perfectly vertical. They just can |
|
|
|
@ -241,7 +241,7 @@ |
|
|
|
current_yi += down_flag; // Line is heading down, we just want to go to the bottom
|
|
|
|
while (current_yi != cell_dest_yi + down_flag) { |
|
|
|
current_yi += dyi; |
|
|
|
const float next_mesh_line_y = LOGICAL_Y_POSITION(pgm_read_float(&(ubl.mesh_index_to_ypos[current_yi]))); |
|
|
|
const float next_mesh_line_y = LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[current_yi])); |
|
|
|
|
|
|
|
/**
|
|
|
|
* if the slope of the line is infinite, we won't do the calculations |
|
|
|
@ -263,7 +263,7 @@ |
|
|
|
*/ |
|
|
|
if (isnan(z0)) z0 = 0.0; |
|
|
|
|
|
|
|
const float y = LOGICAL_Y_POSITION(pgm_read_float(&(ubl.mesh_index_to_ypos[current_yi]))); |
|
|
|
const float y = LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[current_yi])); |
|
|
|
|
|
|
|
/**
|
|
|
|
* Without this check, it is possible for the algorithm to generate a zero length move in the case |
|
|
|
@ -274,7 +274,7 @@ |
|
|
|
if (y != start[Y_AXIS]) { |
|
|
|
if (!inf_normalized_flag) { |
|
|
|
|
|
|
|
//on_axis_distance = y - start[Y_AXIS];
|
|
|
|
//on_axis_distance = y - start[Y_AXIS];
|
|
|
|
on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS]; |
|
|
|
|
|
|
|
//on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
|
|
|
|
@ -283,7 +283,7 @@ |
|
|
|
//on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
|
|
|
|
//on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
|
|
|
|
|
|
|
|
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; |
|
|
|
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; |
|
|
|
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; |
|
|
|
} |
|
|
|
else { |
|
|
|
@ -321,7 +321,7 @@ |
|
|
|
// edge of this cell for the first move.
|
|
|
|
while (current_xi != cell_dest_xi + left_flag) { |
|
|
|
current_xi += dxi; |
|
|
|
const float next_mesh_line_x = LOGICAL_X_POSITION(pgm_read_float(&(ubl.mesh_index_to_xpos[current_xi]))), |
|
|
|
const float next_mesh_line_x = LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[current_xi])), |
|
|
|
y = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
|
|
|
|
|
|
|
|
float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi); |
|
|
|
@ -337,7 +337,7 @@ |
|
|
|
*/ |
|
|
|
if (isnan(z0)) z0 = 0.0; |
|
|
|
|
|
|
|
const float x = LOGICAL_X_POSITION(pgm_read_float(&(ubl.mesh_index_to_xpos[current_xi]))); |
|
|
|
const float x = LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[current_xi])); |
|
|
|
|
|
|
|
/**
|
|
|
|
* Without this check, it is possible for the algorithm to generate a zero length move in the case |
|
|
|
@ -393,8 +393,8 @@ |
|
|
|
|
|
|
|
while (xi_cnt > 0 || yi_cnt > 0) { |
|
|
|
|
|
|
|
const float next_mesh_line_x = LOGICAL_X_POSITION(pgm_read_float(&(ubl.mesh_index_to_xpos[current_xi + dxi]))), |
|
|
|
next_mesh_line_y = LOGICAL_Y_POSITION(pgm_read_float(&(ubl.mesh_index_to_ypos[current_yi + dyi]))), |
|
|
|
const float next_mesh_line_x = LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[current_xi + dxi])), |
|
|
|
next_mesh_line_y = LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[current_yi + dyi])), |
|
|
|
y = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line
|
|
|
|
x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
|
|
|
|
// (No need to worry about m being zero.
|
|
|
|
|
Scott Lahteine
8 years ago
GitHub