3 files changed, 50 insertions, 0 deletions
diff --git a/game/geoData.cpp b/game/geoData.cpp
index 55c69a1..d66f1ed 100644
--- a/game/geoData.cpp
+++ b/game/geoData.cpp
@@ -1,11 +1,14 @@
 #include "geoData.h"
 #include "gfx/image.h"
 #include <algorithm>
+#include <array>
+#include <cmath>
 #include <cstddef>
 #include <maths.h>
 #include <random>
 #include <stb/stb_image.h>
 #include <stdexcept>
+#include <util.h>
 
 GeoData::GeoData(Limits l, float s) :
 	limit {std::move(l)}, size {(limit.second - limit.first) + 1}, scale {s}, nodes {[this]() {
@@ -65,6 +68,26 @@ GeoData::loadFromImages(const std::filesystem::path & fileName, float scale_)
 	});
 }
 
+glm::vec3
+GeoData::positionAt(const glm::vec2 wcoord) const
+{
+	const auto coord {wcoord / scale};
+	const std::array<glm::vec2, 4> gridCorner {
+			{{std::floor(coord.x), std::floor(coord.y)}, {std::floor(coord.x), std::ceil(coord.y)},
+					{std::ceil(coord.x), std::floor(coord.y)}, {std::ceil(coord.x), std::ceil(coord.y)}}};
+
+	const auto point {transform_array(gridCorner, [this](const auto c) {
+		return nodes[at(c)].height;
+	})};
+
+	const glm::vec2 frac = coord - gridCorner.front();
+	const auto heightFloor = point[0] + ((point[2] - point[0]) * frac.x),
+			   heightCeil = point[1] + ((point[3] - point[1]) * frac.x),
+			   heightMid = heightFloor + ((heightCeil - heightFloor) * frac.y);
+
+	return wcoord || heightMid;
+}
+
 unsigned int
 GeoData::at(glm::ivec2 coord) const
 {
diff --git a/game/geoData.h b/game/geoData.h
index 39c149c..e5057c3 100644
--- a/game/geoData.h
+++ b/game/geoData.h
@@ -20,6 +20,8 @@ public:
 	void generateRandom();
 	void loadFromImages(const std::filesystem::path &, float scale);
 
+	[[nodiscard]] glm::vec3 positionAt(glm::vec2) const;
+
 	[[nodiscard]] unsigned int at(glm::ivec2) const;
 	[[nodiscard]] unsigned int at(int x, int y) const;
 
diff --git a/test/test-geo.cpp b/test/test-geo.cpp
index b6f276a..a28b83a 100644
--- a/test/test-geo.cpp
+++ b/test/test-geo.cpp
@@ -69,3 +69,28 @@ BOOST_FIXTURE_TEST_CASE(load_uk_heightmap, TestGeoData)
 		return n.height > 0;
 	}));
 }
+
+BOOST_FIXTURE_TEST_CASE(get_height_at, TestGeoData)
+{
+	// at(x,y) is index based
+	nodes[at(0, 0)].height = 1;
+	nodes[at(1, 0)].height = 2;
+	nodes[at(0, 1)].height = 3;
+	nodes[at(1, 1)].height = 4;
+
+	// positionAt(x,y) is world based
+	// Corners
+	BOOST_CHECK_EQUAL(positionAt({0, 0}), glm::vec3(0, 0, 1));
+	BOOST_CHECK_EQUAL(positionAt({5, 0}), glm::vec3(5, 0, 2));
+	BOOST_CHECK_EQUAL(positionAt({0, 5}), glm::vec3(0, 5, 3));
+	BOOST_CHECK_EQUAL(positionAt({5, 5}), glm::vec3(5, 5, 4));
+
+	// Edge midpoints
+	BOOST_CHECK_EQUAL(positionAt({0, 2.5F}), glm::vec3(0, 2.5F, 2));
+	BOOST_CHECK_EQUAL(positionAt({5, 2.5F}), glm::vec3(5, 2.5F, 3));
+	BOOST_CHECK_EQUAL(positionAt({2.5F, 0}), glm::vec3(2.5F, 0, 1.5F));
+	BOOST_CHECK_EQUAL(positionAt({2.5F, 5}), glm::vec3(2.5F, 5, 3.5F));
+
+	// Centre
+	BOOST_CHECK_EQUAL(positionAt({2.5F, 2.5F}), glm::vec3(2.5F, 2.5F, 2.5F));
+}