diff --git a/TileGenerator.cpp b/TileGenerator.cpp
index 60095f9..3d3bbe0 100644
--- a/TileGenerator.cpp
+++ b/TileGenerator.cpp
@@ -177,7 +177,9 @@ TileGenerator::TileGenerator():
 	m_tileHeight(0),
 	m_tileBorderSize(1),
 	m_tileMapXOffset(0),
-	m_tileMapYOffset(0)
+	m_tileMapYOffset(0),
+	m_surfaceHeight(INT_MIN),
+	m_surfaceDepth(INT_MAX)
 {
 	// Load default grey colors.
 	m_heightMapColors.push_back(HeightMapColor(INT_MIN, Color(0,0,0), -129, Color(0,0,0)));
@@ -298,7 +300,18 @@ void TileGenerator::setDrawPlayers(bool drawPlayers)
 
 void TileGenerator::setDrawScale(int scale)
 {
-	m_drawScale = (scale & DRAWSCALE_MASK);
+	m_drawScale = (scale & DRAWSCALE_MASK) | (m_drawScale & DRAWHEIGHTSCALE_MASK & ((~scale & DRAWSCALE_MASK) << 4));
+}
+
+void TileGenerator::setDrawHeightScale(int scale)
+{
+	unsigned s = scale;
+	int bits = 0;
+	for (; s; s >>= 1)
+		if ((s & 0x1)) bits++;
+	if (bits > 1)
+		throw std::runtime_error(std::string("Multiple height scale positions requested"));
+	m_drawScale = (scale & DRAWHEIGHTSCALE_MASK) | (m_drawScale & DRAWSCALE_MASK & ((~scale & DRAWHEIGHTSCALE_MASK) >> 4));
 }
 
 void TileGenerator::setDrawAlpha(bool drawAlpha)
@@ -436,9 +449,12 @@ void TileGenerator::generate(const std::string &input, const std::string &output
 	computeMapParameters(input);
 	createImage();
 	renderMap();
-	if (m_drawScale) {
+	if ((m_drawScale & DRAWSCALE_MASK)) {
 		renderScale();
 	}
+	if (m_heightMap && (m_drawScale & DRAWHEIGHTSCALE_MASK)) {
+		renderHeightScale();
+	}
 	if (m_drawOrigin) {
 		renderOrigin();
 	}
@@ -1406,6 +1422,29 @@ void TileGenerator::renderMap()
 		cout << std::setw(50) << "" << "\r";
 }
 
+Color TileGenerator::computeMapHeightColor(int height)
+{
+	int adjustedHeight = int((height - m_seaLevel) * m_heightMapYScale + 0.5);
+	float r = 0;
+	float g = 0;
+	float b = 0;
+	int n = 0;
+	for (HeightMapColorList::iterator i = m_heightMapColors.begin(); i != m_heightMapColors.end(); i++) {
+		HeightMapColor &colorSpec = *i;
+		if (adjustedHeight >= colorSpec.height[0] && adjustedHeight <= colorSpec.height[1]) {
+			float weight = (float) (colorSpec.height[1] - adjustedHeight + 1) / (colorSpec.height[1] - colorSpec.height[0] + 1);
+			for (int j = 0; j < 2; j++) {
+				r += colorSpec.color[j].r * weight;
+				g += colorSpec.color[j].g * weight;
+				b += colorSpec.color[j].b * weight;
+				weight = 1 - weight;
+			}
+			n++;
+		}
+	}
+	return Color(int(r / n + 0.5), int(g / n + 0.5), int(b / n + 0.5));
+}
+
 inline void TileGenerator::renderMapBlock(const ustring &mapBlock, const BlockPos &pos, int version)
 {
 	checkBlockNodeDataLimit(version, mapBlock.length());
@@ -1438,26 +1477,12 @@ inline void TileGenerator::renderMapBlock(const ustring &mapBlock, const BlockPo
 				int height = pos.y * 16 + y;
 				if (m_heightMap) {
 					if (m_nodeIDColor[content] && nodeColor.a != 0) {
-						int adjustedHeight = int((height - m_seaLevel) * m_heightMapYScale + 0.5);
-						rowIsEmpty = false;
-						float r = 0;
-						float g = 0;
-						float b = 0;
-						int n = 0;
-						for (HeightMapColorList::iterator i = m_heightMapColors.begin(); i != m_heightMapColors.end(); i++) {
-							HeightMapColor &colorSpec = *i;
-							if (adjustedHeight >= colorSpec.height[0] && adjustedHeight <= colorSpec.height[1]) {
-								float weight = (float) (colorSpec.height[1] - adjustedHeight + 1) / (colorSpec.height[1] - colorSpec.height[0] + 1);
-								for (int j = 0; j < 2; j++) {
-									r += colorSpec.color[j].r * weight;
-									g += colorSpec.color[j].g * weight;
-									b += colorSpec.color[j].b * weight;
-									weight = 1 - weight;
-								}
-								n++;
-							}
+						if (!(m_readedPixels[z] & (1 << x))) {
+							if (height > m_surfaceHeight) m_surfaceHeight = height;
+							if (height < m_surfaceDepth) m_surfaceDepth = height;
 						}
-						pixel = PixelAttribute(Color(int(r / n + 0.5), int(g / n + 0.5), int(b / n + 0.5)), height);
+						rowIsEmpty = false;
+						pixel = PixelAttribute(computeMapHeightColor(height), height);
 						m_readedPixels[z] |= (1 << x);
 						break;
 					}
@@ -1507,7 +1532,7 @@ void TileGenerator::renderScale()
 			gdImageLine(m_image, xPos, 0, xPos, borderTop() - 1, color);
 		}
 	}
-	
+
 	if ((m_drawScale & DRAWSCALE_LEFT)) {
 		for (int i = (m_zMax / 4) * 4; i >= m_zMin; i -= 4) {
 			stringstream buf1, buf2;
@@ -1526,6 +1551,48 @@ void TileGenerator::renderScale()
 	// DRAWSCALE_RIGHT and DRAWSCALE_BOTTOM not implemented - getting the text positioned right seems not trivial (??)
 }
 
+void TileGenerator::renderHeightScale()
+{
+
+	int scaleColor = m_scaleColor.to_libgd();
+	int height_min = m_surfaceDepth - 16;
+	int height_limit = m_surfaceHeight + 16;
+	int xBorderOffset = borderLeft();
+	int yBorderOffset = borderTop() + m_pictHeight;
+	double height_step = (double)(height_limit - height_min) / m_pictWidth;
+	if (height_step < 1.0 / 16) {
+		height_step = 1.0 / 16;
+	}
+	double number_step = 64;
+	while (number_step / height_step < 48)
+		number_step *= 2;
+	while (number_step / height_step > 96)
+		number_step /= 2;
+
+	double height = height_min;
+	for (int x = 0; height < height_limit; x++, height += height_step) {
+		Color color = computeMapHeightColor(int(height + 0.5));
+		gdImageLine(m_image, xBorderOffset + x, yBorderOffset + 8, xBorderOffset + x, yBorderOffset + borderBottom() - 20, color.to_libgd());
+
+		int iheight = int(height + (height > 0 ? 0.5 : -0.5));
+		int iheight64 = int(iheight / number_step + (height > 0 ? 0.5 : -0.5)) * number_step;
+		if (fabs(height - iheight64) <= height_step / 2 && (height - iheight64) > -height_step / 2) {
+			if (iheight64 / int(number_step) % 2 == 1 && fabs(height) > 9999 && number_step / height_step < 56) {
+				// Maybe not enough room for the number. Draw a tick mark instead
+				gdImageLine(m_image, xBorderOffset + x, yBorderOffset + borderBottom() - 19, xBorderOffset + x, yBorderOffset + borderBottom() - 16, scaleColor);
+			}
+			else {
+				stringstream buf;
+				buf << iheight64;
+				string scaleText = buf.str();
+				gdImageString(m_image, gdFontGetMediumBold(), xBorderOffset + x + 2, yBorderOffset + borderBottom() - 16,
+					reinterpret_cast<unsigned char *>(const_cast<char *>(scaleText.c_str())), scaleColor);
+				gdImageLine(m_image, xBorderOffset + x, yBorderOffset + borderBottom() - 19, xBorderOffset + x, yBorderOffset + borderBottom() - 1, scaleColor);
+			}
+		}
+	}
+}
+
 void TileGenerator::renderOrigin()
 {
 	int imageX = worldX2ImageX(0);
diff --git a/TileGenerator.h b/TileGenerator.h
index c2a9f88..6043759 100644
--- a/TileGenerator.h
+++ b/TileGenerator.h
@@ -44,9 +44,15 @@
 #define DRAWSCALE_RIGHT			0x02
 #define DRAWSCALE_TOP			0x04
 #define DRAWSCALE_BOTTOM		0x08
+#define DRAWHEIGHTSCALE_MASK		0xf0
+#define DRAWHEIGHTSCALE_LEFT		0x10
+#define DRAWHEIGHTSCALE_RIGHT		0x20
+#define DRAWHEIGHTSCALE_TOP		0x40
+#define DRAWHEIGHTSCALE_BOTTOM		0x80
 
 #define SCALESIZE_HOR			40
 #define SCALESIZE_VERT			50
+#define HEIGHTSCALESIZE			60
 
 class TileGenerator
 {
@@ -116,6 +122,7 @@ public:
 	void setDrawOrigin(bool drawOrigin);
 	void setDrawPlayers(bool drawPlayers);
 	void setDrawScale(int scale);
+	void setDrawHeightScale(int scale);
 	void setDrawAlpha(bool drawAlpha);
 	void setDrawAir(bool drawAir);
 	void drawObject(const DrawObject &object) { m_drawObjects.push_back(object); }
@@ -138,6 +145,7 @@ public:
 	void setBackend(std::string backend);
 	void setChunkSize(int size);
 	void generate(const std::string &input, const std::string &output);
+	Color computeMapHeightColor(int height);
 
 private:
 	std::string getWorldDatabaseBackend(const std::string &input);
@@ -168,6 +176,7 @@ private:
 	void processMapBlock(const DB::Block &block);
 	void renderMapBlock(const ustring &mapBlock, const BlockPos &pos, int version);
 	void renderScale();
+	void renderHeightScale();
 	void renderOrigin();
 	void renderPlayers(const std::string &inputPath);
 	void renderDrawObjects();
@@ -179,10 +188,10 @@ private:
 	int worldZ2ImageY(int val) const;
 	int worldBlockX2StoredX(int xPos) const { return (xPos - m_xMin) * 16; }
 	int worldBlockZ2StoredY(int zPos) const { return (m_zMax - zPos) * 16; }
-	int borderTop() const { return ((m_drawScale & DRAWSCALE_TOP) ? SCALESIZE_HOR : 0); }
-	int borderBottom() const { return ((m_drawScale & DRAWSCALE_BOTTOM) ? SCALESIZE_HOR : 0); }
-	int borderLeft() const { return ((m_drawScale & DRAWSCALE_LEFT) ? SCALESIZE_VERT : 0); }
-	int borderRight() const { return ((m_drawScale & DRAWSCALE_RIGHT) ? SCALESIZE_VERT : 0); }
+	int borderTop() const { return ((m_drawScale & DRAWSCALE_TOP) ? SCALESIZE_HOR : 0) + (m_heightMap && (m_drawScale & DRAWHEIGHTSCALE_TOP) ? HEIGHTSCALESIZE : 0); }
+	int borderBottom() const { return ((m_drawScale & DRAWSCALE_BOTTOM) ? SCALESIZE_HOR : 0) + (m_heightMap && (m_drawScale & DRAWHEIGHTSCALE_BOTTOM) ? HEIGHTSCALESIZE : 0); }
+	int borderLeft() const { return ((m_drawScale & DRAWSCALE_LEFT) ? SCALESIZE_VERT : 0) + (m_heightMap && (m_drawScale & DRAWHEIGHTSCALE_LEFT) ? HEIGHTSCALESIZE : 0); }
+	int borderRight() const { return ((m_drawScale & DRAWSCALE_RIGHT) ? SCALESIZE_VERT : 0) + (m_heightMap && (m_drawScale & DRAWHEIGHTSCALE_RIGHT) ? HEIGHTSCALESIZE : 0); }
 
 	void parseDataFile(const std::string &fileName, int depth, const char *type,
 		void (TileGenerator::*parseLine)(const std::string &line, std::string name,
@@ -261,6 +270,8 @@ private:
 	int m_tileBorderYCount;
 	int m_pictWidth;
 	int m_pictHeight;
+	int m_surfaceHeight;
+	int m_surfaceDepth;
 	std::list<BlockPos> m_positions;
 	NodeID2NameMap m_nameMap;
 	static const ColorEntry *NodeColorNotDrawn;
diff --git a/mapper.cpp b/mapper.cpp
index 2d65daf..52db549 100644
--- a/mapper.cpp
+++ b/mapper.cpp
@@ -36,6 +36,7 @@ using namespace std;
 #define OPT_HEIGHT_LEVEL0		0x8a
 #define OPT_HEIGHTMAPNODESFILE		0x8b
 #define OPT_HEIGHTMAPCOLORSFILE		0x8c
+#define OPT_DRAWHEIGHTSCALE		0x8d
 
 // Will be replaced with the actual name and location of the executable (if found)
 string executableName = "minetestmapper";
@@ -85,6 +86,7 @@ void usage()
 			"  --origincolor <color>\n"
 			"  --tilebordercolor <color>\n"
 			"  --drawscale[=left,top]\n"
+			"  --drawheightscale\n"
 			"  --drawplayers\n"
 			"  --draworigin\n"
 			"  --drawalpha[=cumulative|cumulative-darken|average|none]\n"
@@ -551,6 +553,7 @@ int main(int argc, char *argv[])
 		{"draworigin", no_argument, 0, 'R'},
 		{"drawplayers", no_argument, 0, 'P'},
 		{"drawscale", optional_argument, 0, 'S'},
+		{"drawheightscale", no_argument, 0, OPT_DRAWHEIGHTSCALE},
 		{"drawalpha", optional_argument, 0, 'e'},
 		{"drawair", no_argument, 0, OPT_DRAWAIR},
 		{"drawpoint", required_argument, 0, OPT_DRAW_OBJECT},
@@ -724,6 +727,9 @@ int main(int argc, char *argv[])
 						generator.setDrawScale(DRAWSCALE_LEFT | DRAWSCALE_TOP);
 					}
 					break;
+				case OPT_DRAWHEIGHTSCALE :
+					generator.setDrawHeightScale(DRAWHEIGHTSCALE_BOTTOM);
+					break;
 				case 'v':
 					if (optarg && isdigit(optarg[0]) && optarg[1] == '\0') {
 						if (optarg[0] == '0')