diff options
Diffstat (limited to 'src/Movement/Grid.cpp')
| -rw-r--r-- | src/Movement/Grid.cpp | 113 |
1 files changed, 98 insertions, 15 deletions
diff --git a/src/Movement/Grid.cpp b/src/Movement/Grid.cpp index d338ceb7..ba194537 100644 --- a/src/Movement/Grid.cpp +++ b/src/Movement/Grid.cpp @@ -6,9 +6,12 @@ */ #include "Grid.h" -#include "Configuration.h" +#include "RepRapFirmware.h" #include <cmath> +// Increase the version number in the following string whenever we change the format of the height map file. +const char *HeightMapComment = "RepRapFirmware height map file v1"; + // Initialise the grid to be invalid GridDefinition::GridDefinition() : xMin(0.0), xMax(-1.0), yMin(0.0), yMax(-1.0), radius(-1.0), spacing(DefaultGridSpacing), gridHeights(nullptr), @@ -17,10 +20,10 @@ GridDefinition::GridDefinition() } GridDefinition::GridDefinition(const float xRange[2], const float yRange[2], float pRadius, float pSpacing) - : xMin(xRange[0]), xMax(xRange[1]), yMin(yRange[0]), yMax(yRange[1]), radius(pRadius), spacing(pSpacing) + : xMin(xRange[0]), xMax(xRange[1]), yMin(yRange[0]), yMax(yRange[1]), radius(pRadius), spacing(pSpacing), recipSpacing(1.0/spacing) { - numX = (xMax - xMin >= MinRange && spacing >= MinSpacing) ? (uint32_t)((xMax - xMin)/spacing) + 1 : 0; - numY = (yMax - yMin >= MinRange && spacing >= MinSpacing) ? (uint32_t)((xMax - xMin)/spacing) + 1 : 0; + numX = (xMax - xMin >= MinRange && spacing >= MinSpacing) ? (uint32_t)((xMax - xMin) * recipSpacing) + 1 : 0; + numY = (yMax - yMin >= MinRange && spacing >= MinSpacing) ? (uint32_t)((yMax - yMin) * recipSpacing) + 1 : 0; isValid = NumPoints() != 0 && NumPoints() <= MaxGridProbePoints && (radius < 0.0 || radius >= 1.0); } @@ -77,6 +80,75 @@ void GridDefinition::PrintError(StringRef& r) const } } +// Save the grid to file returning true if an error occurred +bool GridDefinition::SaveToFile(FileStore *f) const +{ + char bufferSpace[500]; + StringRef buf(bufferSpace, ARRAY_SIZE(bufferSpace)); + + // Write the header comment + buf.copy(HeightMapComment); + if (reprap.GetPlatform()->IsDateTimeSet()) + { + time_t timeNow = reprap.GetPlatform()->GetDateTime(); + const struct tm * const timeInfo = gmtime(&timeNow); + buf.catf(" generated at %04u-%02u-%02u %02u:%02u", + timeInfo->tm_year, timeInfo->tm_mon, timeInfo->tm_mday, timeInfo->tm_hour, timeInfo->tm_min); + } + buf.cat('\n'); + if (!f->Write(buf.Pointer())) + { + return true; + } + + // Write the grid parameters + buf.printf("xmin,xmax,ymin,ymax,radius,spacing,xnum,ynum\n" + "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%u,%u\n", + xMin, xMax, yMin, yMax, radius, spacing, numX, numY + ); + if (!f->Write(buf.Pointer())) + { + return true; + } + + // Write the grid heights + uint32_t index = 0; + for (uint32_t i = 0; i < numY; ++i) + { + buf.Clear(); + for (uint32_t j = 0; j < numX; ++j) + { + if (j != 0) + { + buf.cat(','); + } + if (IsHeightSet(index)) + { + buf.catf("%.3f%", gridHeights[index]); + } + else + { + buf.cat("0"); // write 0 with no decimal point where we didn't probe, so we can tell when we reload it + } + ++index; + } + buf.cat('\n'); + if (!f->Write(buf.Pointer())) + { + return true; + } + } + + return false; +} + +// Load the grid from file returning true if an error occurred +bool GridDefinition::LoadFromFile(FileStore *f) +{ + //TODO + return true; +} + // Compute the height error at the specified point float GridDefinition::ComputeHeightError(float x, float y) const { @@ -185,32 +257,43 @@ float GridDefinition::InterpolateXY(uint32_t xIndex, uint32_t yIndex, float xFra case 2: // (X1,Y0) defined return gridHeights[indexX1Y0]; case 3: // (X0,Y0) and (X1,Y0) defined - return Interpolate2(indexX0Y0, indexX1Y0, xFrac); + return (xFrac * gridHeights[indexX1Y0]) + ((1.0 - xFrac) * gridHeights[indexX0Y0]); case 4: // (X0,Y1) defined return gridHeights[indexX0Y1]; case 5: // (X0,Y0) and (X0,Y1) defined - return Interpolate2(indexX0Y0, indexX0Y1, yFrac); + return (yFrac * gridHeights[indexX0Y1]) + ((1.0 - yFrac) * gridHeights[indexX0Y0]); case 6: // (X1,Y0) and (X0,Y1) defined - diagonal interpolation - return DiagonalInterpolate(indexX1Y0, indexX0Y1, 1.0 - xFrac, yFrac); + return (((xFrac + 1.0 - yFrac) * gridHeights[indexX1Y0]) + ((yFrac + 1.0 - xFrac) * gridHeights[indexX0Y1]))/2; case 7: // (X0,Y0), (X1,Y0) and (X0,Y1) defined - 3-way interpolation - return Interpolate3(indexX0Y0, indexX1Y0, indexX0Y1, xFrac, yFrac); + return InterpolateCorner(indexX0Y0, indexX1Y0, indexX0Y1, xFrac, yFrac); case 8: // (X1,Y1) defined return gridHeights[indexX1Y1]; case 9: // (X0,Y0) and (X1,Y1) defined - diagonal interpolation - return DiagonalInterpolate(indexX0Y0, indexX1Y1, xFrac, yFrac); + return ((xFrac + yFrac) * gridHeights[indexX1Y1]) + ((2.0 - (xFrac + yFrac)) * gridHeights[indexX0Y0])/2; case 10: // (X1,Y0) and (X1,Y1) defined - return Interpolate2(indexX1Y0, indexX1Y1, yFrac); + return (yFrac * gridHeights[indexX1Y1]) + ((1.0 - yFrac) * gridHeights[indexX1Y0]); case 11: // (X0,Y0), (X1,Y0) and (X1,Y1) defined - 3-way interpolation - return Interpolate3(indexX1Y0, indexX0Y0, indexX1Y1, xFrac, yFrac); + return InterpolateCorner(indexX1Y0, indexX0Y0, indexX1Y1, xFrac, yFrac); case 12: // (X0,Y1) and (X1,Y1) defined - return Interpolate2(indexX0Y1, indexX1Y1, yFrac); + return (xFrac * gridHeights[indexX1Y1]) + ((1.0 - xFrac) * gridHeights[indexX0Y1]); case 13: // (X0,Y0), (X0,Y1) and (X1,Y1) defined - 3-way interpolation - return Interpolate3(indexX0Y1, indexX1Y1, indexX0Y0, xFrac, 1.0 - yFrac); + return InterpolateCorner(indexX0Y1, indexX1Y1, indexX0Y0, xFrac, 1.0 - yFrac); case 14: // (X1,Y0), (X0,Y1) and (X1,Y1) defined - 3-way interpolation - return Interpolate3(indexX1Y1, indexX0Y1, indexX1Y0, 1.0 - xFrac, 1.0 - yFrac); + return InterpolateCorner(indexX1Y1, indexX0Y1, indexX1Y0, 1.0 - xFrac, 1.0 - yFrac); case 15: // All points defined - return Interpolate4(indexX0Y0, indexX1Y0, indexX0Y1, indexX1Y1, xFrac, yFrac); + { + const float xyFrac = xFrac * yFrac; + return (gridHeights[indexX0Y0] * (1.0 - xFrac - yFrac + xyFrac)) + + (gridHeights[indexX1Y0] * (xFrac - xyFrac)) + + (gridHeights[indexX0Y1] * (yFrac - xyFrac)) + + (gridHeights[indexX1Y1] * xyFrac); + } } } +float GridDefinition::InterpolateCorner(uint32_t cornerIndex, uint32_t indexX, uint32_t indexY, float xFrac, float yFrac) const +{ + return ((xFrac * gridHeights[indexX]) + (yFrac * gridHeights[indexY]) + ((2.0 - xFrac - yFrac) * gridHeights[cornerIndex]))/2; +} + // End |