Merge pull request #6552 from thinkyhead/rc_more_ubl_cleanup

Further cleanup of UBL
This commit is contained in:
Scott Lahteine 2017-05-02 22:46:04 -05:00 committed by GitHub
commit 628391304f
13 changed files with 456 additions and 483 deletions

View File

@ -258,8 +258,8 @@
: find_closest_circle_to_print(x_pos, y_pos); // Find the closest Mesh Intersection to where we are now. : find_closest_circle_to_print(x_pos, y_pos); // Find the closest Mesh Intersection to where we are now.
if (location.x_index >= 0 && location.y_index >= 0) { if (location.x_index >= 0 && location.y_index >= 0) {
const float circle_x = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_index])), const float circle_x = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
circle_y = pgm_read_float(&(ubl.mesh_index_to_ypos[location.y_index])); circle_y = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
// Let's do a couple of quick sanity checks. We can pull this code out later if we never see it catch a problem // Let's do a couple of quick sanity checks. We can pull this code out later if we never see it catch a problem
#ifdef DELTA #ifdef DELTA
@ -401,8 +401,8 @@
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) { for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
if (!is_bit_set(circle_flags, i, j)) { if (!is_bit_set(circle_flags, i, j)) {
const float mx = pgm_read_float(&(ubl.mesh_index_to_xpos[i])), // We found a circle that needs to be printed const float mx = pgm_read_float(&ubl.mesh_index_to_xpos[i]), // We found a circle that needs to be printed
my = pgm_read_float(&(ubl.mesh_index_to_ypos[j])); my = pgm_read_float(&ubl.mesh_index_to_ypos[j]);
// Get the distance to this intersection // Get the distance to this intersection
float f = HYPOT(X - mx, Y - my); float f = HYPOT(X - mx, Y - my);
@ -446,11 +446,11 @@
// We found two circles that need a horizontal line to connect them // We found two circles that need a horizontal line to connect them
// Print it! // Print it!
// //
sx = pgm_read_float(&(ubl.mesh_index_to_xpos[ i ])) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right edge sx = pgm_read_float(&ubl.mesh_index_to_xpos[ i ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right edge
ex = pgm_read_float(&(ubl.mesh_index_to_xpos[i + 1])) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // left edge ex = pgm_read_float(&ubl.mesh_index_to_xpos[i + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // left edge
sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1); sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1);
sy = ey = constrain(pgm_read_float(&(ubl.mesh_index_to_ypos[j])), Y_MIN_POS + 1, Y_MAX_POS - 1); sy = ey = constrain(pgm_read_float(&ubl.mesh_index_to_ypos[j]), Y_MIN_POS + 1, Y_MAX_POS - 1);
ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1); ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
if (ubl.g26_debug_flag) { if (ubl.g26_debug_flag) {
@ -477,10 +477,10 @@
// We found two circles that need a vertical line to connect them // We found two circles that need a vertical line to connect them
// Print it! // Print it!
// //
sy = pgm_read_float(&(ubl.mesh_index_to_ypos[ j ])) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top edge sy = pgm_read_float(&ubl.mesh_index_to_ypos[ j ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top edge
ey = pgm_read_float(&(ubl.mesh_index_to_ypos[j + 1])) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // bottom edge ey = pgm_read_float(&ubl.mesh_index_to_ypos[j + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // bottom edge
sx = ex = constrain(pgm_read_float(&(ubl.mesh_index_to_xpos[i])), X_MIN_POS + 1, X_MAX_POS - 1); sx = ex = constrain(pgm_read_float(&ubl.mesh_index_to_xpos[i]), X_MIN_POS + 1, X_MAX_POS - 1);
sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1); sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1); ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);

View File

@ -4919,7 +4919,7 @@ void home_all_axes() { gcode_G28(); }
// For LINEAR and 3POINT leveling correct the current position // For LINEAR and 3POINT leveling correct the current position
if (verbose_level > 0) if (verbose_level > 0)
planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:"); planner.bed_level_matrix.debug(PSTR("\n\nBed Level Correction Matrix:"));
if (!dryrun) { if (!dryrun) {
// //
@ -6965,7 +6965,7 @@ inline void gcode_M111() {
for (uint8_t i = 0; i < COUNT(debug_strings); i++) { for (uint8_t i = 0; i < COUNT(debug_strings); i++) {
if (TEST(marlin_debug_flags, i)) { if (TEST(marlin_debug_flags, i)) {
if (comma++) SERIAL_CHAR(','); if (comma++) SERIAL_CHAR(',');
serialprintPGM((char*)pgm_read_word(&(debug_strings[i]))); serialprintPGM((char*)pgm_read_word(&debug_strings[i]));
} }
} }
} }
@ -8360,7 +8360,7 @@ void quickstop_stepper() {
// V to print the matrix or mesh // V to print the matrix or mesh
if (code_seen('V')) { if (code_seen('V')) {
#if ABL_PLANAR #if ABL_PLANAR
planner.bed_level_matrix.debug("Bed Level Correction Matrix:"); planner.bed_level_matrix.debug(PSTR("Bed Level Correction Matrix:"));
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR) #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
if (bilinear_grid_spacing[X_AXIS]) { if (bilinear_grid_spacing[X_AXIS]) {
print_bilinear_leveling_grid(); print_bilinear_leveling_grid();
@ -9545,16 +9545,16 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
tmp_offset_vec.debug("tmp_offset_vec"); tmp_offset_vec.debug(PSTR("tmp_offset_vec"));
act_offset_vec.debug("act_offset_vec"); act_offset_vec.debug(PSTR("act_offset_vec"));
offset_vec.debug("offset_vec (BEFORE)"); offset_vec.debug(PSTR("offset_vec (BEFORE)"));
} }
#endif #endif
offset_vec.apply_rotation(planner.bed_level_matrix.transpose(planner.bed_level_matrix)); offset_vec.apply_rotation(planner.bed_level_matrix.transpose(planner.bed_level_matrix));
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) offset_vec.debug("offset_vec (AFTER)"); if (DEBUGGING(LEVELING)) offset_vec.debug(PSTR("offset_vec (AFTER)"));
#endif #endif
// Adjustments to the current position // Adjustments to the current position

View File

@ -452,7 +452,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled
#define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 17) // mm #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 17) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
#define DELTA_AUTO_CALIBRATION #define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

View File

@ -459,7 +459,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled
#define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 17) // mm #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 17) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
//#define DELTA_AUTO_CALIBRATION //#define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

View File

@ -448,7 +448,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled
#define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 28) // mm #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 28) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
//#define DELTA_AUTO_CALIBRATION //#define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

View File

@ -448,7 +448,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled
#define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 18) // mm #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 18) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
//#define DELTA_AUTO_CALIBRATION //#define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

View File

@ -435,7 +435,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled
#define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 25.4) // mm #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 25.4) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
//#define DELTA_AUTO_CALIBRATION //#define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

View File

@ -453,7 +453,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled
#define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 28) // mm #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 28) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
//#define DELTA_AUTO_CALIBRATION //#define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

View File

@ -66,12 +66,12 @@ int finish_incremental_LSF(struct linear_fit_data *lsf) {
lsf->xbar /= N; lsf->xbar /= N;
lsf->ybar /= N; lsf->ybar /= N;
lsf->zbar /= N; lsf->zbar /= N;
lsf->x2bar = lsf->x2bar / N - lsf->xbar * lsf->xbar; lsf->x2bar = lsf->x2bar / N - sq(lsf->xbar);
lsf->y2bar = lsf->y2bar / N - lsf->ybar * lsf->ybar; lsf->y2bar = lsf->y2bar / N - sq(lsf->ybar);
lsf->z2bar = lsf->z2bar / N - lsf->zbar * lsf->zbar; lsf->z2bar = lsf->z2bar / N - sq(lsf->zbar);
lsf->xybar = lsf->xybar / N - lsf->xbar * lsf->ybar; lsf->xybar = lsf->xybar / N - sq(lsf->xbar);
lsf->yzbar = lsf->yzbar / N - lsf->ybar * lsf->zbar; lsf->yzbar = lsf->yzbar / N - sq(lsf->ybar);
lsf->xzbar = lsf->xzbar / N - lsf->xbar * lsf->zbar; lsf->xzbar = lsf->xzbar / N - sq(lsf->xbar);
const float DD = lsf->x2bar * lsf->y2bar - sq(lsf->xybar); const float DD = lsf->x2bar * lsf->y2bar - sq(lsf->xybar);
if (fabs(DD) <= 1e-10 * (lsf->max_absx + lsf->max_absy)) if (fabs(DD) <= 1e-10 * (lsf->max_absx + lsf->max_absy))

File diff suppressed because it is too large Load Diff

View File

@ -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. * 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 * 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 ]), (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 * 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 // 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. // 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; float z0 = z1 + (z2 - z1) * yratio;
@ -198,8 +198,8 @@
const float dx = end[X_AXIS] - start[X_AXIS], const float dx = end[X_AXIS] - start[X_AXIS],
dy = end[Y_AXIS] - start[Y_AXIS]; dy = end[Y_AXIS] - start[Y_AXIS];
const int left_flag = dx < 0.0 ? 1.0 : 0.0, const int left_flag = dx < 0.0 ? 1 : 0,
down_flag = dy < 0.0 ? 1.0 : 0.0; down_flag = dy < 0.0 ? 1 : 0;
const float adx = left_flag ? -dx : dx, const float adx = left_flag ? -dx : dx,
ady = down_flag ? -dy : dy; ady = down_flag ? -dy : dy;
@ -230,8 +230,8 @@
const float m = dy / dx, const float m = dy / dx,
c = start[Y_AXIS] - m * start[X_AXIS]; c = start[Y_AXIS] - m * start[X_AXIS];
const bool inf_normalized_flag=isinf(e_normalized_dist), const bool inf_normalized_flag = isinf(e_normalized_dist),
inf_m_flag=isinf(m); inf_m_flag = isinf(m);
/** /**
* This block handles vertical lines. These are lines that stay within the same * 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 * 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 current_yi += down_flag; // Line is heading down, we just want to go to the bottom
while (current_yi != cell_dest_yi + down_flag) { while (current_yi != cell_dest_yi + down_flag) {
current_yi += dyi; 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 * if the slope of the line is infinite, we won't do the calculations
@ -263,7 +263,7 @@
*/ */
if (isnan(z0)) z0 = 0.0; 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 * 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 (y != start[Y_AXIS]) {
if (!inf_normalized_flag) { 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 ? 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]; //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 ? 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]; //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; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
} }
else { else {
@ -321,7 +321,7 @@
// edge of this cell for the first move. // edge of this cell for the first move.
while (current_xi != cell_dest_xi + left_flag) { while (current_xi != cell_dest_xi + left_flag) {
current_xi += dxi; 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 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); 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; 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 * 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) { 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]))), 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]))), 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 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 x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
// (No need to worry about m being zero. // (No need to worry about m being zero.

View File

@ -63,7 +63,7 @@ vector_3 vector_3::get_normal() {
return normalized; return normalized;
} }
float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); } float vector_3::get_length() { return sqrt(sq(x) + sq(y) + sq(z)); }
void vector_3::normalize() { void vector_3::normalize() {
const float inv_length = 1.0 / get_length(); const float inv_length = 1.0 / get_length();
@ -81,8 +81,8 @@ void vector_3::apply_rotation(matrix_3x3 matrix) {
z = resultZ; z = resultZ;
} }
void vector_3::debug(const char title[]) { void vector_3::debug(const char * const title) {
SERIAL_PROTOCOL(title); serialprintPGM(title);
SERIAL_PROTOCOLPGM(" x: "); SERIAL_PROTOCOLPGM(" x: ");
SERIAL_PROTOCOL_F(x, 6); SERIAL_PROTOCOL_F(x, 6);
SERIAL_PROTOCOLPGM(" y: "); SERIAL_PROTOCOLPGM(" y: ");
@ -101,14 +101,14 @@ void apply_rotation_xyz(matrix_3x3 matrix, float &x, float &y, float &z) {
} }
matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) { matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
//row_0.debug("row_0"); //row_0.debug(PSTR("row_0"));
//row_1.debug("row_1"); //row_1.debug(PSTR("row_1"));
//row_2.debug("row_2"); //row_2.debug(PSTR("row_2"));
matrix_3x3 new_matrix; matrix_3x3 new_matrix;
new_matrix.matrix[0] = row_0.x; new_matrix.matrix[1] = row_0.y; new_matrix.matrix[2] = row_0.z; new_matrix.matrix[0] = row_0.x; new_matrix.matrix[1] = row_0.y; new_matrix.matrix[2] = row_0.z;
new_matrix.matrix[3] = row_1.x; new_matrix.matrix[4] = row_1.y; new_matrix.matrix[5] = row_1.z; new_matrix.matrix[3] = row_1.x; new_matrix.matrix[4] = row_1.y; new_matrix.matrix[5] = row_1.z;
new_matrix.matrix[6] = row_2.x; new_matrix.matrix[7] = row_2.y; new_matrix.matrix[8] = row_2.z; new_matrix.matrix[6] = row_2.x; new_matrix.matrix[7] = row_2.y; new_matrix.matrix[8] = row_2.z;
//new_matrix.debug("new_matrix"); //new_matrix.debug(PSTR("new_matrix"));
return new_matrix; return new_matrix;
} }
@ -123,14 +123,14 @@ matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal(); vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal();
vector_3 y_row = vector_3::cross(z_row, x_row).get_normal(); vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
// x_row.debug("x_row"); // x_row.debug(PSTR("x_row"));
// y_row.debug("y_row"); // y_row.debug(PSTR("y_row"));
// z_row.debug("z_row"); // z_row.debug(PSTR("z_row"));
// create the matrix already correctly transposed // create the matrix already correctly transposed
matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row); matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
// rot.debug("rot"); // rot.debug(PSTR("rot"));
return rot; return rot;
} }
@ -142,8 +142,8 @@ matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
return new_matrix; return new_matrix;
} }
void matrix_3x3::debug(const char title[]) { void matrix_3x3::debug(const char * const title) {
SERIAL_PROTOCOLLN(title); serialprintPGM(title);
uint8_t count = 0; uint8_t count = 0;
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
for (uint8_t j = 0; j < 3; j++) { for (uint8_t j = 0; j < 3; j++) {

View File

@ -42,6 +42,7 @@
#define VECTOR_3_H #define VECTOR_3_H
#if HAS_ABL #if HAS_ABL
class matrix_3x3; class matrix_3x3;
struct vector_3 { struct vector_3 {
@ -58,7 +59,7 @@ struct vector_3 {
float get_length(); float get_length();
vector_3 get_normal(); vector_3 get_normal();
void debug(const char title[]); void debug(const char * const title);
void apply_rotation(matrix_3x3 matrix); void apply_rotation(matrix_3x3 matrix);
}; };
@ -72,11 +73,11 @@ struct matrix_3x3 {
void set_to_identity(); void set_to_identity();
void debug(const char title[]); void debug(const char * const title);
}; };
void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z); void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z);
#endif // HAS_ABL
#endif // HAS_ABL
#endif // VECTOR_3_H #endif // VECTOR_3_H