From bc0ab4d9e88d80ccc442dd9de036b637df153a03 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jorge=20Tim=C3=B3n?= Date: Wed, 7 Sep 2022 11:46:37 +0100 Subject: [PATCH] Hangprinter: Generalize IsReachable() to any pseudo-pyramid This is a clean generalization from 4 to N. Pseudo-pyramid because the vertices in the base of the pseudo-pyramid don't need to be on the same plane. There's no base of the pyramid as such. Better documented and cleaner thanks to @tobbelobb on github. --- .../Kinematics/HangprinterKinematics.cpp | 26 +++++++++++++------ 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/src/Movement/Kinematics/HangprinterKinematics.cpp b/src/Movement/Kinematics/HangprinterKinematics.cpp index 48eafcda1e..4c8e9efc58 100644 --- a/src/Movement/Kinematics/HangprinterKinematics.cpp +++ b/src/Movement/Kinematics/HangprinterKinematics.cpp @@ -419,17 +419,27 @@ static bool isSameSide(float const v0[3], float const v1[3], float const v2[3], return dot0*dot1 > 0.0F; } -static bool isInsideTetrahedron(float const point[3], float const tetrahedron[4][3]){ - return isSameSide(tetrahedron[0], tetrahedron[1], tetrahedron[2], tetrahedron[3], point) && - isSameSide(tetrahedron[2], tetrahedron[1], tetrahedron[3], tetrahedron[0], point) && - isSameSide(tetrahedron[2], tetrahedron[3], tetrahedron[0], tetrahedron[1], point) && - isSameSide(tetrahedron[0], tetrahedron[3], tetrahedron[1], tetrahedron[2], point); -} - +// For each triangle side in a pseudo-pyramid, check if the point is inside the pyramid (Except for the base) +// Also check that any point below the line between two exterior anchors (all anchors are exterior except for the last one) +// is in the "inside part" all the way down to min_Z, however low it may be. +// To further limit the movements in the X and Y axes one can simply set a smaller print radius. bool HangprinterKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes) const noexcept /*override*/ { float const coords[3] = {axesCoords[X_AXIS], axesCoords[Y_AXIS], axesCoords[Z_AXIS]}; - return isInsideTetrahedron(coords, anchors); + bool reachable = true; + + // Check all the planes defined by triangle sides in the pyramid + for (size_t i = 0; reachable && i < HANGPRINTER_AXES - 1; ++i) { + reachable = reachable && isSameSide(anchors[i], anchors[(i+1) % (HANGPRINTER_AXES - 1)], anchors[HANGPRINTER_AXES - 1], anchors[(i+2) % (HANGPRINTER_AXES - 1)], coords); + } + + // For each side of the base, check the plane formed by side and another point bellow them in z. + for (size_t i = 0; reachable && i < HANGPRINTER_AXES - 1; ++i) { + float const lower_point[3] = {anchors[i][0], anchors[i][1], anchors[i][2] - 1}; + reachable = reachable && isSameSide(anchors[i], anchors[(i+1) % (HANGPRINTER_AXES - 1)], lower_point, anchors[(i+2) % (HANGPRINTER_AXES - 1)], coords); + } + + return reachable; } // Limit the Cartesian position that the user wants to move to returning true if we adjusted the position