From cccd8246abd42d790012ce3b0f7287317378b1cd Mon Sep 17 00:00:00 2001
From: Marek Bel <marek.bel@posta.cz>
Date: Mon, 23 Apr 2018 20:15:33 +0200
Subject: [PATCH] Fix world2machine(const float &x, const float &y, float
 &out_x, float &out_y) not using input parameters if only
 WORLD2MACHINE_CORRECTION_SHIFT is applied.

---
 Firmware/Marlin_main.cpp        | 11 +++++++++++
 Firmware/mesh_bed_calibration.h | 27 +++++++--------------------
 2 files changed, 18 insertions(+), 20 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 48182e10..585c2557 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -3107,7 +3107,18 @@ void process_commands()
               feedrate = homing_feedrate[Z_AXIS]/10;
               current_position[Z_AXIS] = 0;
               enable_endstops(false);
+#ifdef DEBUG_BUILD
+              SERIAL_ECHOLNPGM("plan_set_position()");
+              MYSERIAL.println(current_position[X_AXIS]);MYSERIAL.println(current_position[Y_AXIS]);
+              MYSERIAL.println(current_position[Z_AXIS]);MYSERIAL.println(current_position[E_AXIS]);
+#endif
               plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+#ifdef DEBUG_BUILD
+              SERIAL_ECHOLNPGM("plan_buffer_line()");
+              MYSERIAL.println(destination[X_AXIS]);MYSERIAL.println(destination[Y_AXIS]);
+              MYSERIAL.println(destination[Z_AXIS]);MYSERIAL.println(destination[E_AXIS]);
+              MYSERIAL.println(feedrate);MYSERIAL.println(active_extruder);
+#endif
               plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
               st_synchronize();
               current_position[X_AXIS] = destination[X_AXIS];
diff --git a/Firmware/mesh_bed_calibration.h b/Firmware/mesh_bed_calibration.h
index 5dd2202e..349397a9 100644
--- a/Firmware/mesh_bed_calibration.h
+++ b/Firmware/mesh_bed_calibration.h
@@ -37,26 +37,6 @@ extern void world2machine_initialize();
 // to current_position[x,y].
 extern void world2machine_update_current();
 
-inline void world2machine(const float &x, const float &y, float &out_x, float &out_y)
-{
-	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {
-		// No correction.
-		out_x = x;
-		out_y = y;
-	} else {
-		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SKEW) {
-			// Firs the skew & rotation correction.
-			out_x = world2machine_rotation_and_skew[0][0] * x + world2machine_rotation_and_skew[0][1] * y;
-			out_y = world2machine_rotation_and_skew[1][0] * x + world2machine_rotation_and_skew[1][1] * y;
-		}
-		if (world2machine_correction_mode & WORLD2MACHINE_CORRECTION_SHIFT) {
-			// Then add the offset.
-			out_x += world2machine_shift[0];
-			out_y += world2machine_shift[1];
-		}
-	}
-}
-
 inline void world2machine(float &x, float &y)
 {
 	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {
@@ -77,6 +57,13 @@ inline void world2machine(float &x, float &y)
 	}
 }
 
+inline void world2machine(const float &x, const float &y, float &out_x, float &out_y)
+{
+    out_x = x;
+    out_y = y;
+    world2machine(out_x, out_y);
+}
+
 inline void machine2world(float x, float y, float &out_x, float &out_y)
 {
 	if (world2machine_correction_mode == WORLD2MACHINE_CORRECTION_NONE) {