3.5 Hangprinter: Generalize IsReachable() to any pseudo-pyramid

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