MPCNC/Firmware
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Fix the planeNormal calculation

Browse Source
This commit is contained in:
Scott Lahteine 2015-03-26 17:14:07 -07:00
parent 0b32431551
commit b76a352d41
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Marlin/Marlin_main.cpp
View File

@ -1110,11 +1110,13 @@ static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float
vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1); vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2); vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3); vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal();
vector_3 from_2_to_1 = (pt1 - pt2).get_normal(); if (planeNormal.z < 0) {
vector_3 from_2_to_3 = (pt3 - pt2).get_normal(); planeNormal.x = -planeNormal.x;
vector_3 planeNormal = vector_3::cross(from_2_to_1, from_2_to_3).get_normal(); planeNormal.y = -planeNormal.y;
planeNormal = vector_3(planeNormal.x, planeNormal.y, planeNormal.z); planeNormal.z = -planeNormal.z;
}
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal); plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);