Correction and tidy-up of ChristophPech's PR

Moved call to ExtrapolateMissing in grid.cpp to what I believe is the
correct place
Tidied up code to fix compiler warnnigs and confirm to style guidelines

4 files changed, 165 insertions, 116 deletions

diff --git a/src/Movement/Grid.cpp b/src/Movement/Grid.cpp
index 7e7bfbd1..2b86f5e4 100644
--- a/src/Movement/Grid.cpp
+++ b/src/Movement/Grid.cpp
@@ -282,10 +282,9 @@ bool HeightMap::LoadFromFile(FileStore *f, StringRef& r)
 				}
 			}
 		}
+		ExtrapolateMissing();
 		return false;										// success!
 	}
-
-	ExtrapolateMissing();
 	return true;											// an error occurred
 }
 
@@ -329,11 +328,11 @@ float HeightMap::GetInterpolatedHeightError(float x, float y) const
 		return 0.0;
 	}
 
-	//last grid point
+	// Last grid point
 	const float xLast = def.xMin + (def.numX-1)*def.spacing;
 	const float yLast = def.yMin + (def.numY-1)*def.spacing;
 
-	//clamp to rectangle so InterpolateXY will always have valid parameters
+	// Clamp to rectangle so InterpolateXY will always have valid parameters
 	const float fEPSILON = 0.01;
 	if (x < def.xMin) { x = def.xMin; }
 	if (y < def.yMin) {	y = def.yMin; }
@@ -373,81 +372,85 @@ void HeightMap::ExtrapolateMissing()
 	//algorithm: http://www.ilikebigbits.com/blog/2015/3/2/plane-from-points
 	float sumX = 0, sumY = 0, sumZ = 0;
 	int n = 0;
-	for (int iY = 0; iY < def.numY; iY++)
+	for (uint32_t iY = 0; iY < def.numY; iY++)
 	{
-		for (int iX = 0; iX < def.numX; iX++)
+		for (uint32_t iX = 0; iX < def.numX; iX++)
 		{
-			int index = GetMapIndex(iX, iY);
-			if (!IsHeightSet(index)) { continue; }
-			float fX = (def.spacing * iX) + def.xMin;
-			float fY = (def.spacing * iY) + def.yMin;
-			float fZ = gridHeights[index];
-
-			n++;
-			sumX += fX; sumY += fY; sumZ += fZ;
+			const uint32_t index = GetMapIndex(iX, iY);
+			if (IsHeightSet(index))
+			{
+				const float fX = (def.spacing * iX) + def.xMin;
+				const float fY = (def.spacing * iY) + def.yMin;
+				const float fZ = gridHeights[index];
+
+				n++;
+				sumX += fX; sumY += fY; sumZ += fZ;
+			}
 		}
 	}
 
-	float invN = 1.0 / float(n);
-	float centX = sumX * invN, centY = sumY * invN, centZ = sumZ * invN;
+	const float invN = 1.0 / float(n);
+	const float centX = sumX * invN, centY = sumY * invN, centZ = sumZ * invN;
 
 	// Calc full 3x3 covariance matrix, excluding symmetries:
 	float xx = 0.0; float xy = 0.0; float xz = 0.0;
 	float yy = 0.0; float yz = 0.0; float zz = 0.0;
 
-	for (int iY = 0; iY < def.numY; iY++)
+	for (uint32_t iY = 0; iY < def.numY; iY++)
 	{
-		for (int iX = 0; iX < def.numX; iX++)
+		for (uint32_t iX = 0; iX < def.numX; iX++)
 		{
-			int index = GetMapIndex(iX, iY);
-			if (!IsHeightSet(index)) { continue; }
-			float fX = (def.spacing * iX) + def.xMin;
-			float fY = (def.spacing * iY) + def.yMin;
-			float fZ = gridHeights[index];
-
-			float rX = fX - centX;
-			float rY = fY - centY;
-			float rZ = fZ - centZ;
-
-			xx += rX * rX;
-			xy += rX * rY;
-			xz += rX * rZ;
-			yy += rY * rY;
-			yz += rY * rZ;
-			zz += rZ * rZ;
+			const uint32_t index = GetMapIndex(iX, iY);
+			if (IsHeightSet(index))
+			{
+				const float fX = (def.spacing * iX) + def.xMin;
+				const float fY = (def.spacing * iY) + def.yMin;
+				const float fZ = gridHeights[index];
+
+				const float rX = fX - centX;
+				const float rY = fY - centY;
+				const float rZ = fZ - centZ;
+
+				xx += rX * rX;
+				xy += rX * rY;
+				xz += rX * rZ;
+				yy += rY * rY;
+				yz += rY * rZ;
+				zz += rZ * rZ;
+			}
 		}
 	}
 
 	const float detZ = xx*yy - xy*xy;
-	if (detZ <= 0) {
-		//not a valid plane (or a vertical one)
+	if (detZ <= 0)
+	{
+		// Not a valid plane (or a vertical one)
 		return;
 	}
 
-	//plane equation: ax+by+cz=d -> z = (d-(ax+by))/c
+	// Plane equation: ax+by+cz=d -> z = (d-(ax+by))/c
 	float a = (yz*xy - xz*yy) / detZ;
 	float b = (xz*xy - yz*xx) / detZ;
-	float c = 1.0;
-	float normLenInv=1.0/sqrtf(a*a + b*b + 1);
+	const float invC = sqrtf(a*a + b*b + 1.0);
+	const float normLenInv = 1.0 / invC;
 	a *= normLenInv;
 	b *= normLenInv;
-	c *= normLenInv;
-	float d = centX*a + centY*b + centZ*c;
-
+	const float c = normLenInv;
+	const float d = centX*a + centY*b + centZ*c;
 
-	//fill in the blanks
-	float invC = 1.0 / c;
-	for (int iY = 0; iY < def.numY; iY++)
+	// Fill in the blanks
+	for (uint32_t iY = 0; iY < def.numY; iY++)
 	{
-		for (int iX = 0; iX < def.numX; iX++)
+		for (uint32_t iX = 0; iX < def.numX; iX++)
 		{
-			int index = GetMapIndex(iX, iY);
-			if (IsHeightSet(index)) { continue; }
-			float fX = (def.spacing * iX) + def.xMin;
-			float fY = (def.spacing * iY) + def.yMin;
-
-			float fZ = (d - (a * fX + b * fY)) * invC;
-			gridHeights[index] = fZ;	//fill in Z but don't mark it as set so we can always differentiate between measured and extrapolated
+			const uint32_t index = GetMapIndex(iX, iY);
+			if (!IsHeightSet(index))
+			{
+				const float fX = (def.spacing * iX) + def.xMin;
+				const float fY = (def.spacing * iY) + def.yMin;
+				const float fZ = (d - (a * fX + b * fY)) * invC;
+				gridHeights[index] = fZ;	// fill in Z but don't mark it as set so we can always differentiate between measured and extrapolated
+			}
 		}
 	}
 }
diff --git a/src/Movement/Grid.h b/src/Movement/Grid.h
index a1189018..84d8b589 100644
--- a/src/Movement/Grid.h
+++ b/src/Movement/Grid.h
@@ -81,7 +81,7 @@ public:
 
 	unsigned int GetStatistics(float& mean, float& deviation) const; // Return number of points probed, mean and RMS deviation
 
-	void ExtrapolateMissing();										//extrapolate missing points to ensure consistency
+	void ExtrapolateMissing();										// Extrapolate missing points to ensure consistency
 
 private:
 	static const char *HeightMapComment;							// The start of the comment we write at the start of the height map file
diff --git a/src/Movement/Move.cpp b/src/Movement/Move.cpp
index 1e535845..f831c0c3 100644
--- a/src/Movement/Move.cpp
+++ b/src/Movement/Move.cpp
@@ -184,8 +184,7 @@ void Move::Spin()
 				// We have a new move
 				if (simulationMode < 2)		// in simulation mode 2 and higher, we don't process incoming moves beyond this point
 				{
-
-					const bool doMotorMapping = (nextMove.moveType == 0) || (nextMove.moveType == 1 && !IsDeltaMode());
+					const bool doMotorMapping = (nextMove.moveType == 0);
 					if (doMotorMapping)
 					{
 						Transform(nextMove.coords, nextMove.xAxes, true);
@@ -936,76 +935,122 @@ void Move::FinishedBedProbing(int sParam, StringRef& reply)
 	}
 }
 
-void Move::SetProbedBedEquation(size_t numPoints, StringRef& reply)
+// Check that the probe points are in the right order
+bool Move::GoodProbePointOrdering(size_t numPoints) const
 {
-	switch(numPoints)
+	if (numPoints >= 2 && yBedProbePoints[1] <= yBedProbePoints[0])
 	{
-	case 3:
-		/*
-		 * Transform to a plane
-		 */
+		return false;
+	}
+	if (numPoints >= 3 && xBedProbePoints[2] <= xBedProbePoints[1])
+	{
+		return false;
+	}
+	if (numPoints >= 4 && yBedProbePoints[3] >= yBedProbePoints[2])
+	{
+		return false;
+	}
+	if (numPoints >= 4 && xBedProbePoints[0] >= xBedProbePoints[3])
+	{
+		return false;
+	}
+	if (numPoints >= 5
+		&& (   xBedProbePoints[4] <= xBedProbePoints[0]
+			|| xBedProbePoints[4] <= xBedProbePoints[1]
+			|| xBedProbePoints[4] >= xBedProbePoints[2]
+			|| xBedProbePoints[4] >= xBedProbePoints[3]
+			|| yBedProbePoints[4] <= yBedProbePoints[0]
+			|| yBedProbePoints[4] >= yBedProbePoints[1]
+			|| yBedProbePoints[4] >= yBedProbePoints[2]
+			|| yBedProbePoints[4] <= yBedProbePoints[3]
+		   )
+	   )
+	{
+		return false;
+	}
+	return true;
+}
 
+void Move::SetProbedBedEquation(size_t numPoints, StringRef& reply)
+{
+	if (!GoodProbePointOrdering(numPoints))
+	{
+		reply.printf("Error: probe points P0 to P%u must be in clockwise order starting near X=0 Y=0", min<unsigned int>(numPoints, 4) - 1);
+		if (numPoints >= 5)
 		{
-			float x10 = xBedProbePoints[1] - xBedProbePoints[0];
-			float y10 = yBedProbePoints[1] - yBedProbePoints[0];
-			float z10 = zBedProbePoints[1] - zBedProbePoints[0];
-			float x20 = xBedProbePoints[2] - xBedProbePoints[0];
-			float y20 = yBedProbePoints[2] - yBedProbePoints[0];
-			float z20 = zBedProbePoints[2] - zBedProbePoints[0];
-			float a = y10 * z20 - z10 * y20;
-			float b = z10 * x20 - x10 * z20;
-			float c = x10 * y20 - y10 * x20;
-			float d = -(xBedProbePoints[1] * a + yBedProbePoints[1] * b + zBedProbePoints[1] * c);
-			aX = -a / c;
-			aY = -b / c;
-			aC = -d / c;
+			reply.cat(" and P4 must be near the centre");
 		}
-		break;
+	}
+	else
+	{
+		switch(numPoints)
+		{
+		case 3:
+			/*
+			 * Transform to a plane
+			 */
+			{
+				float x10 = xBedProbePoints[1] - xBedProbePoints[0];
+				float y10 = yBedProbePoints[1] - yBedProbePoints[0];
+				float z10 = zBedProbePoints[1] - zBedProbePoints[0];
+				float x20 = xBedProbePoints[2] - xBedProbePoints[0];
+				float y20 = yBedProbePoints[2] - yBedProbePoints[0];
+				float z20 = zBedProbePoints[2] - zBedProbePoints[0];
+				float a = y10 * z20 - z10 * y20;
+				float b = z10 * x20 - x10 * z20;
+				float c = x10 * y20 - y10 * x20;
+				float d = -(xBedProbePoints[1] * a + yBedProbePoints[1] * b + zBedProbePoints[1] * c);
+				aX = -a / c;
+				aY = -b / c;
+				aC = -d / c;
+			}
+			break;
 
-	case 4:
-		/*
-		 * Transform to a ruled-surface quadratic.  The corner points for interpolation are indexed:
-		 *
-		 *   ^  [1]      [2]
-		 *   |
-		 *   Y
-		 *   |
-		 *   |  [0]      [3]
-		 *      -----X---->
-		 *
-		 *   These are the scaling factors to apply to x and y coordinates to get them into the
-		 *   unit interval [0, 1].
-		 */
-		xRectangle = 1.0 / (xBedProbePoints[3] - xBedProbePoints[0]);
-		yRectangle = 1.0 / (yBedProbePoints[1] - yBedProbePoints[0]);
-		break;
+		case 4:
+			/*
+			 * Transform to a ruled-surface quadratic.  The corner points for interpolation are indexed:
+			 *
+			 *   ^  [1]      [2]
+			 *   |
+			 *   Y
+			 *   |
+			 *   |  [0]      [3]
+			 *      -----X---->
+			 *
+			 *   These are the scaling factors to apply to x and y coordinates to get them into the
+			 *   unit interval [0, 1].
+			 */
+			xRectangle = 1.0 / (xBedProbePoints[3] - xBedProbePoints[0]);
+			yRectangle = 1.0 / (yBedProbePoints[1] - yBedProbePoints[0]);
+			break;
 
-	case 5:
-		for (size_t i = 0; i < 4; i++)
-		{
-			float x10 = xBedProbePoints[i] - xBedProbePoints[4];
-			float y10 = yBedProbePoints[i] - yBedProbePoints[4];
-			float z10 = zBedProbePoints[i] - zBedProbePoints[4];
-			baryXBedProbePoints[i] = xBedProbePoints[4] + 2.0 * x10;
-			baryYBedProbePoints[i] = yBedProbePoints[4] + 2.0 * y10;
-			baryZBedProbePoints[i] = zBedProbePoints[4] + 2.0 * z10;
-		}
-		baryXBedProbePoints[4] = xBedProbePoints[4];
-		baryYBedProbePoints[4] = yBedProbePoints[4];
-		baryZBedProbePoints[4] = zBedProbePoints[4];
-		break;
+		case 5:
+			for (size_t i = 0; i < 4; i++)
+			{
+				float x10 = xBedProbePoints[i] - xBedProbePoints[4];
+				float y10 = yBedProbePoints[i] - yBedProbePoints[4];
+				float z10 = zBedProbePoints[i] - zBedProbePoints[4];
+				baryXBedProbePoints[i] = xBedProbePoints[4] + 2.0 * x10;
+				baryYBedProbePoints[i] = yBedProbePoints[4] + 2.0 * y10;
+				baryZBedProbePoints[i] = zBedProbePoints[4] + 2.0 * z10;
+			}
+			baryXBedProbePoints[4] = xBedProbePoints[4];
+			baryYBedProbePoints[4] = yBedProbePoints[4];
+			baryZBedProbePoints[4] = zBedProbePoints[4];
+			break;
 
-	default:
-		reprap.GetPlatform()->MessageF(GENERIC_MESSAGE, "Bed calibration error: %d points provided but only 3, 4 and 5 supported\n", numPoints);
-		return;
-	}
+		default:
+			reprap.GetPlatform()->MessageF(GENERIC_MESSAGE, "Bed calibration error: %d points provided but only 3, 4 and 5 supported\n", numPoints);
+			return;
+		}
 
-    numBedCompensationPoints = numPoints;
+		numBedCompensationPoints = numPoints;
 
-	reply.copy("Bed equation fits points");
-	for (size_t point = 0; point < numPoints; point++)
-	{
-		reply.catf(" [%.1f, %.1f, %.3f]", xBedProbePoints[point], yBedProbePoints[point], zBedProbePoints[point]);
+		reply.copy("Bed equation fits points");
+		for (size_t point = 0; point < numPoints; point++)
+		{
+			reply.catf(" [%.1f, %.1f, %.3f]", xBedProbePoints[point], yBedProbePoints[point], zBedProbePoints[point]);
+		}
 	}
 	reply.cat("\n");
 }
diff --git a/src/Movement/Move.h b/src/Movement/Move.h
index fcfef155..14b3b804 100644
--- a/src/Movement/Move.h
+++ b/src/Movement/Move.h
@@ -125,6 +125,7 @@ private:
 	enum class IdleState : uint8_t { idle, busy, timing };
 
 	bool StartNextMove(uint32_t startTime);											// start the next move, returning true if Step() needs to be called immediately
+	bool GoodProbePointOrdering(size_t numPoints) const;							// Check that the probe points are in the right order
 	void SetProbedBedEquation(size_t numPoints, StringRef& reply);					// When we have a full set of probed points, work out the bed's equation
 	void DoDeltaCalibration(size_t numPoints, StringRef& reply);
 	void BedTransform(float move[MAX_AXES], uint32_t xAxes) const;					// Take a position and apply the bed compensations