Merge branch 'terrain-deform-2'HEADmain

6 files changed, 353 insertions, 366 deletions

diff --git a/game/geoData.cpp b/game/geoData.cpp
index d212651..a5fc4ef 100644
--- a/game/geoData.cpp
+++ b/game/geoData.cpp
@@ -66,7 +66,7 @@ GeoData::loadFromAsciiGrid(const std::filesystem::path & input)
 			});
 		}
 	}
-	mesh.update_vertex_normals_only();
+	mesh.updateAllVertexNormals();
 
 	return mesh;
 };
@@ -105,7 +105,7 @@ GeoData::createFlat(GlobalPosition2D lower, GlobalPosition2D upper, GlobalDistan
 		}
 	}
 
-	mesh.update_vertex_normals_only();
+	mesh.updateAllVertexNormals();
 
 	return mesh;
 }
@@ -361,7 +361,7 @@ GeoData::findBoundaryStart() const
 [[nodiscard]] RelativePosition3D
 GeoData::difference(const HalfedgeHandle heh) const
 {
-	return point(to_vertex_handle(heh)) - point(from_vertex_handle(heh));
+	return ::difference(point(to_vertex_handle(heh)), point(from_vertex_handle(heh)));
 }
 
 [[nodiscard]] RelativeDistance
@@ -377,23 +377,28 @@ GeoData::centre(const HalfedgeHandle heh) const
 }
 
 void
-GeoData::update_vertex_normals_only()
+GeoData::updateAllVertexNormals()
 {
-	update_vertex_normals_only(vertices_sbegin());
+	updateAllVertexNormals(vertices());
 }
 
+template<std::ranges::range R>
 void
-GeoData::update_vertex_normals_only(VertexIter start)
+GeoData::updateAllVertexNormals(const R & range)
 {
-	std::for_each(start, vertices_end(), [this](const auto vh) {
-		if (normal(vh) == Normal3D {}) {
-			Normal3D n;
-			calc_vertex_normal_correct(vh, n);
-			this->set_normal(vh, glm::normalize(n));
-		}
+	std::ranges::for_each(range, [this](const auto vertex) {
+		updateVertexNormal(vertex);
 	});
 }
 
+void
+GeoData::updateVertexNormal(VertexHandle vertex)
+{
+	Normal3D n;
+	calc_vertex_normal_correct(vertex, n);
+	set_normal(vertex, glm::normalize(n));
+}
+
 bool
 GeoData::triangleOverlapsTriangle(const Triangle<2> & a, const Triangle<2> & b)
 {
@@ -411,265 +416,161 @@ GeoData::triangleContainsTriangle(const Triangle<2> & a, const Triangle<2> & b)
 }
 
 void
-GeoData::split(FaceHandle _fh)
-{
-	// Collect halfedges of face
-	const HalfedgeHandle he0 = halfedge_handle(_fh);
-	const HalfedgeHandle he1 = next_halfedge_handle(he0);
-	const HalfedgeHandle he2 = next_halfedge_handle(he1);
-
-	const EdgeHandle eh0 = edge_handle(he0);
-	const EdgeHandle eh1 = edge_handle(he1);
-	const EdgeHandle eh2 = edge_handle(he2);
-
-	// Collect points of face
-	const VertexHandle p0 = to_vertex_handle(he0);
-	const VertexHandle p1 = to_vertex_handle(he1);
-	const VertexHandle p2 = to_vertex_handle(he2);
-
-	// Calculate midpoint coordinates
-	const Point new0 = centre(he0);
-	const Point new1 = centre(he1);
-	const Point new2 = centre(he2);
-
-	// Add vertices at midpoint coordinates
-	const VertexHandle v0 = add_vertex(new0);
-	const VertexHandle v1 = add_vertex(new1);
-	const VertexHandle v2 = add_vertex(new2);
-
-	const bool split0 = !is_boundary(eh0);
-	const bool split1 = !is_boundary(eh1);
-	const bool split2 = !is_boundary(eh2);
-
-	// delete original face
-	delete_face(_fh, false);
-
-	// split boundary edges of deleted face ( if not boundary )
-	if (split0) {
-		split(eh0, v0);
-	}
-
-	if (split1) {
-		split(eh1, v1);
-	}
-
-	if (split2) {
-		split(eh2, v2);
-	}
-
-	// Retriangulate
-	add_face(v0, p0, v1);
-	add_face(p2, v0, v2);
-	add_face(v2, v1, p1);
-	add_face(v2, v0, v1);
-}
-
-void
-GeoData::setHeights(const std::span<const GlobalPosition3D> triangleStrip, const Surface & newFaceSurface)
+GeoData::setHeights(const std::span<const GlobalPosition3D> triangleStrip, const SetHeightsOpts & opts)
 {
-	static const RelativeDistance MAX_SLOPE = 1.5F;
-	static const RelativeDistance MIN_ARC = 0.01F;
-
 	if (triangleStrip.size() < 3) {
 		return;
 	}
+	const auto stripMinMax = std::ranges::minmax(triangleStrip, {}, &GlobalPosition3D::z);
+	lowerExtent.z = std::min(upperExtent.z, stripMinMax.min.z);
+	upperExtent.z = std::max(upperExtent.z, stripMinMax.max.z);
 
-	const auto initialVertexCount = static_cast<unsigned int>(n_vertices());
+	const auto vertexDistFrom = [this](GlobalPosition2D p) {
+		return [p, this](const VertexHandle v) {
+			return std::make_pair(v, glm::length(::difference(p, this->point(v).xy())));
+		};
+	};
+
+	std::set<VertexHandle> newOrChangedVerts;
+	auto addVertexForNormalUpdate = [this, &newOrChangedVerts](const VertexHandle vertex) {
+		newOrChangedVerts.emplace(vertex);
+		std::ranges::copy(vv_range(vertex), std::inserter(newOrChangedVerts, newOrChangedVerts.end()));
+	};
 
-	// Create new vertices
+	// New vertices for each vertex in triangleStrip
 	std::vector<VertexHandle> newVerts;
 	newVerts.reserve(newVerts.size());
-	std::transform(triangleStrip.begin(), triangleStrip.end(), std::back_inserter(newVerts), [this](const auto tsVert) {
-		return add_vertex(tsVert);
-	});
-	//  Create new faces
-	const auto initialFaceCount = static_cast<int>(n_faces());
-	std::for_each(strip_begin(newVerts), strip_end(newVerts), [this](const auto & newVert) {
-		const auto [a, b, c] = newVert;
-		add_face(a, b, c);
-	});
-	for (auto fhi = FaceIter {*this, FaceHandle {initialFaceCount}, true}; fhi != faces_end(); fhi++) {
-		static constexpr auto MAX_FACE_AREA = 100'000'000.F;
-		const auto fh = *fhi;
-		if (triangle<3>(fh).area() > MAX_FACE_AREA) {
-			split(fh);
-		}
-	}
-	std::vector<FaceHandle> newFaces;
-	std::copy_if(FaceIter {*this, FaceHandle {initialFaceCount}, true}, faces_end(), std::back_inserter(newFaces),
-			[this](FaceHandle fh) {
-				return !this->status(fh).deleted();
+	std::transform(triangleStrip.begin(), triangleStrip.end(), std::back_inserter(newVerts),
+			[this, &newVerts, &vertexDistFrom, &opts](const auto tsPoint) {
+				const auto face = findPoint(tsPoint);
+				if (const auto nearest = std::ranges::min(std::views::iota(fv_begin(face), fv_end(face))
+									| std::views::transform(vertexDistFrom(tsPoint)),
+							{}, &std::pair<VertexHandle, float>::second);
+						nearest.second < opts.nearNodeTolerance && !std::ranges::contains(newVerts, nearest.first)) {
+					point(nearest.first) = tsPoint;
+					return nearest.first;
+				}
+				return split(face, tsPoint);
 			});
-
-	// Extrude corners
-	struct Extrusion {
-		VertexHandle boundaryVertex, extrusionVertex;
-		Direction3D lowerLimit, upperLimit;
+	std::ranges::for_each(newVerts, addVertexForNormalUpdate);
+
+	// Create temporary triangles from triangleStrip
+	std::vector<Triangle<3>> strip;
+	std::transform(
+			strip_begin(triangleStrip), strip_end(triangleStrip), std::back_inserter(strip), [](const auto & newVert) {
+				const auto [a, b, c] = newVert;
+				return Triangle<3> {a, b, c};
+			});
+	auto getTriangle = [&strip](const auto point) -> const Triangle<3> * {
+		if (const auto t = std::ranges::find_if(strip,
+					[point](const auto & triangle) {
+						return triangleContainsPoint(point, triangle);
+					});
+				t != strip.end()) {
+			return &*t;
+		}
+		return nullptr;
 	};
 
-	std::vector<Extrusion> extrusionExtents;
-	boundaryWalk(
-			[this, &extrusionExtents](const auto boundaryHeh) {
-				const auto prevBoundaryHeh = prev_halfedge_handle(boundaryHeh);
-				const auto prevBoundaryVertex = from_vertex_handle(prevBoundaryHeh);
-				const auto boundaryVertex = from_vertex_handle(boundaryHeh);
-				const auto nextBoundaryVertex = to_vertex_handle(boundaryHeh);
-				const auto p0 = point(prevBoundaryVertex);
-				const auto p1 = point(boundaryVertex);
-				const auto p2 = point(nextBoundaryVertex);
-				const auto e0 = glm::normalize(vector_normal(RelativePosition2D(p1 - p0)));
-				const auto e1 = glm::normalize(vector_normal(RelativePosition2D(p2 - p1)));
-
-				const auto addExtrusionFor = [this, &extrusionExtents, boundaryVertex, p1](Direction2D direction) {
-					const auto doExtrusion = [this](VertexHandle & extrusionVertex, Direction2D direction,
-													 GlobalPosition3D boundaryVertex, RelativeDistance vert) {
-						const auto extrusionDir = glm::normalize(direction || vert);
-
-						if (!extrusionVertex.is_valid()) {
-							if (const auto intersect = intersectRay({boundaryVertex, extrusionDir})) {
-								auto splitVertex = split(intersect->second, intersect->first);
-								extrusionVertex = splitVertex;
-							}
-							else if (const auto intersect
-									= intersectRay({boundaryVertex + GlobalPosition3D {1, 1, 0}, extrusionDir})) {
-								auto splitVertex = split(intersect->second, intersect->first);
-								extrusionVertex = splitVertex;
-							}
-							else if (const auto intersect
-									= intersectRay({boundaryVertex + GlobalPosition3D {1, 0, 0}, extrusionDir})) {
-								auto splitVertex = split(intersect->second, intersect->first);
-								extrusionVertex = splitVertex;
-							}
-						}
-
-						return extrusionDir;
-					};
-
-					VertexHandle extrusionVertex;
-					extrusionExtents.emplace_back(boundaryVertex, extrusionVertex,
-							doExtrusion(extrusionVertex, direction, p1, -MAX_SLOPE),
-							doExtrusion(extrusionVertex, direction, p1, MAX_SLOPE));
-					assert(extrusionVertex.is_valid());
-				};
-				if (const Arc arc {e0, e1}; arc.length() < MIN_ARC) {
-					addExtrusionFor(normalize(e0 + e1) / cosf(arc.length() / 2.F));
-				}
-				else if (arc.length() < pi) {
-					// Previous half edge end to current half end start arc tangents
-					const auto limit = std::ceil(arc.length() * 5.F / pi);
-					const auto inc = arc.length() / limit;
-					for (float step = 0; step <= limit; step += 1.F) {
-						addExtrusionFor(sincos(arc.first + (step * inc)));
-					}
-				}
-				else {
-					// Single tangent bisecting the difference
-					addExtrusionFor(normalize(e0 + e1) / sinf((arc.length() - pi) / 2.F));
-				}
-			},
-			*voh_begin(newVerts.front()));
-
-	//  Cut existing terrain
-	extrusionExtents.emplace_back(extrusionExtents.front()); // Circular next
-	std::vector<std::vector<VertexHandle>> boundaryFaces;
-	for (const auto & [first, second] : extrusionExtents | std::views::adjacent<2>) {
-		const auto p0 = point(first.boundaryVertex);
-		const auto p1 = point(second.boundaryVertex);
-		const auto bdir = RelativePosition3D(p1 - p0);
-		const auto make_plane = [p0](auto y, auto z) {
-			return GeometricPlaneT<GlobalPosition3D> {p0, crossProduct(y, z)};
-		};
-		const auto planes = ((first.boundaryVertex == second.boundaryVertex)
-						? std::array {make_plane(second.lowerLimit, first.lowerLimit),
-									make_plane(second.upperLimit, first.upperLimit),
-						}
-						: std::array {
-								  make_plane(bdir, second.lowerLimit),
-									make_plane(bdir, second.upperLimit),
-						});
-		assert(planes.front().normal.z > 0.F);
-		assert(planes.back().normal.z > 0.F);
-
-		auto & out = boundaryFaces.emplace_back();
-		out.emplace_back(first.boundaryVertex);
-		out.emplace_back(first.extrusionVertex);
-		for (auto currentVertex = first.extrusionVertex;
-				!find_halfedge(currentVertex, second.extrusionVertex).is_valid();) {
-			[[maybe_unused]] const auto n
-					= std::any_of(voh_begin(currentVertex), voh_end(currentVertex), [&](const auto currentVertexOut) {
-						  const auto next = next_halfedge_handle(currentVertexOut);
-						  const auto nextVertex = to_vertex_handle(next);
-						  const auto startVertex = from_vertex_handle(next);
-						  if (nextVertex == *++out.rbegin()) {
-							  // This half edge goes back to the previous vertex
-							  return false;
-						  }
-						  const auto edge = edge_handle(next);
-						  const auto ep0 = point(startVertex);
-						  const auto ep1 = point(nextVertex);
-						  if (planes.front().getRelation(ep1) == GeometricPlane::PlaneRelation::Below
-								  || planes.back().getRelation(ep1) == GeometricPlane::PlaneRelation::Above) {
-							  return false;
-						  }
-						  const auto diff = RelativePosition3D(ep1 - ep0);
-						  const auto length = glm::length(diff);
-						  const auto dir = diff / length;
-						  const Ray r {ep1, -dir};
-						  const auto dists = planes * [r](const auto & plane) {
-							  RelativeDistance dist {};
-							  if (r.intersectPlane(plane.origin, plane.normal, dist)) {
-								  return dist;
-							  }
-							  return INFINITY;
-						  };
-						  const auto dist = *std::min_element(dists.begin(), dists.end());
-						  const auto splitPos = ep1 - (dir * dist);
-						  if (dist <= length) {
-							  currentVertex = split(edge, splitPos);
-							  out.emplace_back(currentVertex);
-							  return true;
-						  }
-						  return false;
-					  });
-			assert(n);
+	// Cut along each edge of triangleStrip AB, AC, BC, BD, CD, CE etc
+	std::map<VertexHandle, const Triangle<3> *> boundaryTriangles;
+	auto doBoundaryPart = [this, &boundaryTriangles, &newVerts, &vertexDistFrom, &opts, &addVertexForNormalUpdate](
+								  VertexHandle start, VertexHandle end, const Triangle<3> & triangle) {
+		boundaryTriangles.emplace(start, &triangle);
+		const auto endPoint = point(end);
+		while (!std::ranges::contains(vv_range(start), end)
+				&& std::ranges::any_of(voh_range(start), [&](const auto & outHalf) {
+					   const auto next = next_halfedge_handle(outHalf);
+					   const auto startPoint = point(start);
+					   const auto nexts = std::array {from_vertex_handle(next), to_vertex_handle(next)};
+					   const auto nextPoints = nexts | std::views::transform([this](const auto v) {
+						   return std::make_pair(v, this->point(v));
+					   });
+					   if (linesCross(startPoint, endPoint, nextPoints.front().second, nextPoints.back().second)) {
+						   if (const auto intersection = linesIntersectAt(startPoint.xy(), endPoint.xy(),
+									   nextPoints.front().second.xy(), nextPoints.back().second.xy())) {
+							   if (const auto nextDist
+									   = std::ranges::min(nexts | std::views::transform(vertexDistFrom(*intersection)),
+											   {}, &std::pair<VertexHandle, float>::second);
+									   nextDist.second < opts.nearNodeTolerance
+									   && !boundaryTriangles.contains(nextDist.first)
+									   && !std::ranges::contains(newVerts, nextDist.first)) {
+								   start = nextDist.first;
+								   point(start) = positionOnTriangle(*intersection, triangle);
+							   }
+							   else {
+								   start = split(edge_handle(next), positionOnTriangle(*intersection, triangle));
+							   }
+							   addVertexForNormalUpdate(start);
+							   boundaryTriangles.emplace(start, &triangle);
+							   return true;
+						   }
+					   }
+					   return false;
+				   })) { }
+	};
+	auto doBoundary = [&doBoundaryPart, triangle = strip.begin()](const auto & verts) mutable {
+		const auto & [a, b, c] = verts;
+		doBoundaryPart(a, b, *triangle);
+		doBoundaryPart(a, c, *triangle);
+		triangle++;
+	};
+	std::ranges::for_each(newVerts | std::views::adjacent<3>, doBoundary);
+	doBoundaryPart(*++newVerts.rbegin(), newVerts.back(), *strip.rbegin());
+
+	std::set<HalfedgeHandle> done;
+	std::set<HalfedgeHandle> todo;
+	auto todoOutHalfEdges = [&todo, &done, this](const VertexHandle v) {
+		std::copy_if(voh_begin(v), voh_end(v), std::inserter(todo, todo.end()), [&done](const auto & h) {
+			return !done.contains(h);
+		});
+	};
+	std::ranges::for_each(newVerts, todoOutHalfEdges);
+	while (!todo.empty()) {
+		const auto heh = todo.extract(todo.begin()).value();
+		const auto fromVertex = from_vertex_handle(heh);
+		const auto toVertex = to_vertex_handle(heh);
+		const auto & fromPoint = point(fromVertex);
+		auto & toPoint = point(toVertex);
+		auto toTriangle = getTriangle(toPoint);
+		if (!toTriangle) {
+			if (const auto boundaryVertex = boundaryTriangles.find(toVertex);
+					boundaryVertex != boundaryTriangles.end()) {
+				toTriangle = boundaryVertex->second;
+			}
 		}
-		out.emplace_back(second.extrusionVertex);
-		if (first.boundaryVertex != second.boundaryVertex) {
-			out.emplace_back(second.boundaryVertex);
+		if (toTriangle) { // point within the new strip, adjust vertically by triangle
+			toPoint.z = positionOnTriangle(toPoint, *toTriangle).z;
+			addVertexForNormalUpdate(toVertex);
+			todoOutHalfEdges(toVertex);
 		}
+		else if (!toTriangle) { // point without the new strip, adjust vertically by limit
+			const auto maxOffset = static_cast<GlobalDistance>(opts.maxSlope * glm::length(difference(heh).xy()));
+			const auto newHeight = std::clamp(toPoint.z, fromPoint.z - maxOffset, fromPoint.z + maxOffset);
+			if (newHeight != toPoint.z) {
+				toPoint.z = newHeight;
+				addVertexForNormalUpdate(toVertex);
+				std::copy_if(voh_begin(toVertex), voh_end(toVertex), std::inserter(todo, todo.end()),
+						[this, &boundaryTriangles](const auto & heh) {
+							return !boundaryTriangles.contains(to_vertex_handle(heh));
+						});
+			}
+		}
+		done.insert(heh);
 	}
 
-	//  Remove old faces
-	std::set<FaceHandle> visited;
-	auto removeOld = [&](auto & self, const auto face) -> void {
-		if (visited.insert(face).second) {
-			std::for_each(fh_begin(face), fh_end(face), [&](const auto fh) {
-				const auto b1 = to_vertex_handle(fh);
-				const auto b2 = from_vertex_handle(fh);
-				if (opposite_face_handle(fh).is_valid()
-						&& std::none_of(boundaryFaces.begin(), boundaryFaces.end(), [b2, b1](const auto & bf) {
-							   return std::adjacent_find(bf.begin(), bf.end(), [b2, b1](const auto v1, const auto v2) {
-								   return b1 == v1 && b2 == v2;
-							   }) != bf.end();
-						   })) {
-					self(self, opposite_face_handle(fh));
-				}
-			});
-
-			delete_face(face, false);
+	auto surfaceStripWalk = [this, &getTriangle, &opts](const auto & surfaceStripWalk, const auto & face) -> void {
+		if (!property(surface, face)) {
+			property(surface, face) = &opts.surface;
+			std::ranges::for_each(
+					ff_range(face), [this, &getTriangle, &surfaceStripWalk](const auto & adjacentFaceHandle) {
+						if (getTriangle(this->triangle<2>(adjacentFaceHandle).centroid())) {
+							surfaceStripWalk(surfaceStripWalk, adjacentFaceHandle);
+						}
+					});
 		}
 	};
-	removeOld(removeOld, findPoint(triangleStrip.front()));
-
-	std::for_each(boundaryFaces.begin(), boundaryFaces.end(), [&](auto & boundaryFace) {
-		std::reverse(boundaryFace.begin(), boundaryFace.end());
-		add_face(boundaryFace);
-	});
-
-	// Tidy up
-	update_vertex_normals_only(VertexIter {*this, vertex_handle(initialVertexCount), true});
+	surfaceStripWalk(surfaceStripWalk, findPoint(strip.front().centroid()));
 
-	std::for_each(newFaces.begin(), newFaces.end(), [&newFaceSurface, this](const auto fh) {
-		property(surface, fh) = &newFaceSurface;
-	});
+	updateAllVertexNormals(newOrChangedVerts);
 }
diff --git a/game/geoData.h b/game/geoData.h
index ed1734c..79924d3 100644
--- a/game/geoData.h
+++ b/game/geoData.h
@@ -4,6 +4,7 @@
 #include "config/types.h"
 #include "ray.h"
 #include "surface.h"
+#include "triangle.h"
 #include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
 #include <filesystem>
 #include <glm/vec2.hpp>
@@ -52,76 +53,7 @@ public:
 		mutable FaceHandle _face {};
 	};
 
-	template<glm::length_t Dim> struct Triangle : public glm::vec<3, glm::vec<Dim, GlobalDistance>> {
-		using base = glm::vec<3, glm::vec<Dim, GlobalDistance>>;
-		using base::base;
-
-		template<IterableCollection Range> Triangle(const GeoData * m, Range range)
-		{
-			assert(std::distance(range.begin(), range.end()) == 3);
-			std::transform(range.begin(), range.end(), &base::operator[](0), [m](auto vh) {
-				return m->point(vh);
-			});
-		}
-
-		[[nodiscard]] glm::vec<Dim, GlobalDistance>
-		operator*(BaryPosition bari) const
-		{
-			return p(0) + (difference(p(0), p(1)) * bari.x) + (difference(p(0), p(2)) * bari.y);
-		}
-
-		[[nodiscard]] auto
-		area() const
-			requires(Dim == 3)
-		{
-			return glm::length(crossProduct(difference(p(0), p(1)), difference(p(0), p(2)))) / 2.F;
-		}
-
-		[[nodiscard]] Normal3D
-		normal() const
-			requires(Dim == 3)
-		{
-			return crossProduct(difference(p(0), p(1)), difference(p(0), p(2)));
-		}
-
-		[[nodiscard]] Normal3D
-		nnormal() const
-			requires(Dim == 3)
-		{
-			return glm::normalize(normal());
-		}
-
-		[[nodiscard]] auto
-		angle(glm::length_t c) const
-		{
-			return Arc {P(c), P(c + 2), P(c + 1)}.length();
-		}
-
-		template<glm::length_t D = Dim>
-		[[nodiscard]] auto
-		angleAt(const GlobalPosition<D> pos) const
-			requires(D <= Dim)
-		{
-			for (glm::length_t i {}; i < 3; ++i) {
-				if (GlobalPosition<D> {p(i)} == pos) {
-					return angle(i);
-				}
-			}
-			return 0.F;
-		}
-
-		[[nodiscard]] inline auto
-		p(const glm::length_t i) const
-		{
-			return base::operator[](i);
-		}
-
-		[[nodiscard]] inline auto
-		P(const glm::length_t i) const
-		{
-			return base::operator[](i % 3);
-		}
-	};
+	template<glm::length_t Dim> using Triangle = ::Triangle<Dim, GlobalDistance>;
 
 	[[nodiscard]] FaceHandle findPoint(GlobalPosition2D) const;
 	[[nodiscard]] FaceHandle findPoint(GlobalPosition2D, FaceHandle start) const;
@@ -142,7 +74,16 @@ public:
 
 	[[nodiscard]] HalfedgeHandle findEntry(const GlobalPosition2D from, const GlobalPosition2D to) const;
 
-	void setHeights(const std::span<const GlobalPosition3D> triangleStrip, const Surface &);
+	struct SetHeightsOpts {
+		static constexpr auto DEFAULT_NEAR_NODE_TOLERANACE = 500.F;
+		static constexpr auto DEFAULT_MAX_SLOPE = 0.5F;
+
+		const Surface & surface;
+		RelativeDistance nearNodeTolerance = DEFAULT_NEAR_NODE_TOLERANACE;
+		RelativeDistance maxSlope = DEFAULT_MAX_SLOPE;
+	};
+
+	void setHeights(std::span<const GlobalPosition3D> triangleStrip, const SetHeightsOpts &);
 
 	[[nodiscard]] auto
 	getExtents() const
@@ -160,9 +101,13 @@ public:
 protected:
 	template<glm::length_t Dim>
 	[[nodiscard]] Triangle<Dim>
-	triangle(FaceHandle f) const
+	triangle(FaceHandle face) const
 	{
-		return {this, fv_range(f)};
+		Triangle<Dim> triangle;
+		std::ranges::transform(fv_range(face), triangle.begin(), [this](auto vertex) {
+			return point(vertex);
+		});
+		return triangle;
 	}
 
 	[[nodiscard]] static bool triangleContainsPoint(const GlobalPosition2D, const Triangle<2> &);
@@ -172,21 +117,12 @@ protected:
 	[[nodiscard]] HalfedgeHandle findBoundaryStart() const;
 	[[nodiscard]] RelativePosition3D difference(const HalfedgeHandle) const;
 
-	template<glm::length_t D>
-	[[nodiscard]] static RelativePosition<D>
-	difference(const GlobalPosition<D> a, const GlobalPosition<D> b)
-	{
-		return b - a;
-	}
-
 	[[nodiscard]] RelativeDistance length(const HalfedgeHandle) const;
 	[[nodiscard]] GlobalPosition3D centre(const HalfedgeHandle) const;
 
-	void update_vertex_normals_only();
-	void update_vertex_normals_only(VertexIter start);
-
-	using OpenMesh::TriMesh_ArrayKernelT<GeoDataTraits>::split;
-	void split(FaceHandle);
+	void updateAllVertexNormals();
+	template<std::ranges::range R> void updateAllVertexNormals(const R &);
+	void updateVertexNormal(VertexHandle);
 
 private:
 	GlobalPosition3D lowerExtent {}, upperExtent {};
diff --git a/lib/maths.h b/lib/maths.h
index 14a29d1..3959896 100644
--- a/lib/maths.h
+++ b/lib/maths.h
@@ -5,11 +5,15 @@
 #include <glm/glm.hpp>
 #include <glm/gtc/constants.hpp>
 #include <numeric>
+#include <optional>
 #include <stdexcept>
 #include <utility>
 
+template<typename T>
+concept Arithmetic = std::is_arithmetic_v<T>;
+
 struct Arc : public std::pair<Angle, Angle> {
-	template<glm::length_t Lc, glm::length_t Le, typename T, glm::qualifier Q>
+	template<glm::length_t Lc, glm::length_t Le, Arithmetic T, glm::qualifier Q>
 		requires(Lc >= 2, Le >= 2)
 	Arc(const glm::vec<Lc, T, Q> & centre, const glm::vec<Le, T, Q> & e0p, const glm::vec<Le, T, Q> & e1p) :
 		Arc {RelativePosition2D {e0p.xy() - centre.xy()}, RelativePosition2D {e1p.xy() - centre.xy()}}
@@ -118,7 +122,7 @@ namespace {
 	}
 
 	// Helper to lookup into a matrix given an xy vector coordinate
-	template<glm::length_t C, glm::length_t R, typename T, glm::qualifier Q, std::integral I = glm::length_t>
+	template<glm::length_t C, glm::length_t R, Arithmetic T, glm::qualifier Q, std::integral I = glm::length_t>
 	constexpr auto &
 	operator^(glm::mat<C, R, T, Q> & matrix, const glm::vec<2, I> rowCol)
 	{
@@ -200,7 +204,7 @@ rotate_yp(const glm::vec<2, T, Q> & angles)
 	return rotate_yp<D>(angles.y, angles.x);
 }
 
-template<glm::length_t D, glm::qualifier Q = glm::qualifier::defaultp, typename T>
+template<glm::length_t D, glm::qualifier Q = glm::qualifier::defaultp, std::floating_point T>
 	requires(D >= 2)
 constexpr auto
 vector_yaw(const glm::vec<D, T, Q> & diff)
@@ -208,7 +212,7 @@ vector_yaw(const glm::vec<D, T, Q> & diff)
 	return std::atan2(diff.x, diff.y);
 }
 
-template<glm::length_t D, glm::qualifier Q = glm::qualifier::defaultp, typename T>
+template<glm::length_t D, glm::qualifier Q = glm::qualifier::defaultp, std::floating_point T>
 	requires(D >= 3)
 constexpr auto
 vector_pitch(const glm::vec<D, T, Q> & diff)
@@ -216,7 +220,7 @@ vector_pitch(const glm::vec<D, T, Q> & diff)
 	return std::atan(diff.z);
 }
 
-template<typename T, glm::qualifier Q>
+template<std::floating_point T, glm::qualifier Q>
 constexpr glm::vec<2, T, Q>
 vector_normal(const glm::vec<2, T, Q> & vector)
 {
@@ -230,7 +234,7 @@ round_frac(const T value, const T frac)
 	return std::round(value / frac) * frac;
 }
 
-template<typename T>
+template<Arithmetic T>
 	requires requires(T value) { value * value; }
 constexpr auto
 sq(T value)
@@ -263,14 +267,15 @@ crossProduct(const glm::vec<3, T, Q> & valueA, const glm::vec<3, T, Q> & valueB)
 	return glm::cross(valueA, valueB);
 }
 
-template<typename R = float, typename Ta, typename Tb>
+template<Arithmetic R = float, Arithmetic Ta, Arithmetic Tb>
 constexpr auto
 ratio(const Ta valueA, const Tb valueB)
 {
-	return (static_cast<R>(valueA) / static_cast<R>(valueB));
+	using Common = std::common_type_t<Ta, Ta>;
+	return static_cast<R>((static_cast<Common>(valueA) / static_cast<Common>(valueB)));
 }
 
-template<typename R = float, typename T, glm::qualifier Q>
+template<Arithmetic R = float, Arithmetic T, glm::qualifier Q>
 constexpr auto
 ratio(const glm::vec<2, T, Q> & value)
 {
@@ -284,14 +289,14 @@ perspective_divide(const glm::vec<4, T, Q> & value)
 	return value / value.w;
 }
 
-template<glm::length_t L1, glm::length_t L2, typename T, glm::qualifier Q>
+template<glm::length_t L1, glm::length_t L2, Arithmetic T, glm::qualifier Q>
 constexpr glm::vec<L1 + L2, T, Q>
 operator||(const glm::vec<L1, T, Q> valueA, const glm::vec<L2, T, Q> valueB)
 {
 	return {valueA, valueB};
 }
 
-template<glm::length_t L, typename T, glm::qualifier Q>
+template<glm::length_t L, Arithmetic T, glm::qualifier Q>
 constexpr glm::vec<L + 1, T, Q>
 operator||(const glm::vec<L, T, Q> valueA, const T valueB)
 {
@@ -326,7 +331,35 @@ normalize(T ang)
 	return ang;
 }
 
-template<typename T, glm::qualifier Q>
+template<Arithmetic T> using CalcType = std::conditional_t<std::is_floating_point_v<T>, T, int64_t>;
+
+template<Arithmetic T, glm::qualifier Q = glm::defaultp>
+[[nodiscard]] constexpr std::optional<glm::vec<2, T, Q>>
+linesIntersectAt(const glm::vec<2, T, Q> Aabs, const glm::vec<2, T, Q> Babs, const glm::vec<2, T, Q> Cabs,
+		const glm::vec<2, T, Q> Dabs)
+{
+	using CT = CalcType<T>;
+	using CVec = glm::vec<2, CT, Q>;
+	// Line AB represented as a1x + b1y = c1
+	const CVec Brel = Babs - Aabs;
+	const CT a1 = Brel.y;
+	const CT b1 = -Brel.x;
+
+	// Line CD represented as a2x + b2y = c2
+	const CVec Crel = Cabs - Aabs, Del = Dabs - Aabs;
+	const CT a2 = Del.y - Crel.y;
+	const CT b2 = Crel.x - Del.x;
+	const CT c2 = (a2 * Crel.x) + (b2 * Crel.y);
+
+	const auto determinant = (a1 * b2) - (a2 * b1);
+
+	if (determinant == 0) {
+		return std::nullopt;
+	}
+	return Aabs + CVec {(b1 * c2) / -determinant, (a1 * c2) / determinant};
+}
+
+template<Arithmetic T, glm::qualifier Q>
 std::pair<glm::vec<2, T, Q>, bool>
 find_arc_centre(glm::vec<2, T, Q> start, Rotation2D startDir, glm::vec<2, T, Q> end, Rotation2D endDir)
 {
@@ -339,7 +372,7 @@ find_arc_centre(glm::vec<2, T, Q> start, Rotation2D startDir, glm::vec<2, T, Q>
 	throw std::runtime_error("no intersection");
 }
 
-template<typename T, glm::qualifier Q>
+template<Arithmetic T, glm::qualifier Q>
 std::pair<glm::vec<2, T, Q>, bool>
 find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye)
 {
@@ -349,7 +382,7 @@ find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, An
 	return find_arc_centre(start, sincos(entrys + half_pi), end, sincos(entrye - half_pi));
 }
 
-template<typename T, glm::qualifier Q>
+template<Arithmetic T, glm::qualifier Q>
 Angle
 find_arcs_radius(glm::vec<2, T, Q> start, Rotation2D ad, glm::vec<2, T, Q> end, Rotation2D bd)
 {
@@ -374,7 +407,7 @@ find_arcs_radius(glm::vec<2, T, Q> start, Rotation2D ad, glm::vec<2, T, Q> end,
 			/ (2 * (sq(X) - 2 * X * Z + sq(Z) + sq(Y) - 2 * Y * W + sq(W) - 4));
 }
 
-template<typename T, glm::qualifier Q>
+template<Arithmetic T, glm::qualifier Q>
 std::pair<Angle, Angle>
 find_arcs_radius(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye)
 {
@@ -384,7 +417,7 @@ find_arcs_radius(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, A
 	return {getrad(-half_pi), getrad(half_pi)};
 }
 
-template<typename T>
+template<Arithmetic T>
 auto
 midpoint(const std::pair<T, T> & v)
 {
@@ -392,7 +425,7 @@ midpoint(const std::pair<T, T> & v)
 }
 
 // std::pow is not constexpr
-template<typename T>
+template<Arithmetic T>
 	requires requires(T n) { n *= n; }
 constexpr T
 pow(const T base, std::integral auto exp)
@@ -405,14 +438,14 @@ pow(const T base, std::integral auto exp)
 }
 
 // Conversions
-template<typename T>
+template<Arithmetic T>
 constexpr auto
 mph_to_ms(T v)
 {
 	return v / 2.237L;
 }
 
-template<typename T>
+template<Arithmetic T>
 constexpr auto
 kph_to_ms(T v)
 {
diff --git a/lib/triangle.h b/lib/triangle.h
new file mode 100644
index 0000000..812bfab
--- /dev/null
+++ b/lib/triangle.h
@@ -0,0 +1,108 @@
+#pragma once
+
+#include "config/types.h"
+#include "maths.h"
+#include <glm/glm.hpp>
+
+template<glm::length_t Dim, Arithmetic T, glm::qualifier Q = glm::defaultp>
+struct Triangle : public glm::vec<3, glm::vec<Dim, T, Q>> {
+	using Point = glm::vec<Dim, T, Q>;
+	using Base = glm::vec<3, glm::vec<Dim, T, Q>>;
+	using Base::Base;
+
+	[[nodiscard]] constexpr Point
+	operator*(BaryPosition bari) const
+	{
+		return p(0) + (sideDifference(1) * bari.x) + (sideDifference(2) * bari.y);
+	}
+
+	[[nodiscard]] constexpr Point
+	centroid() const
+	{
+		return [this]<glm::length_t... Axis>(std::integer_sequence<glm::length_t, Axis...>) {
+			return Point {(p(0)[Axis] + p(1)[Axis] + p(2)[Axis]) / 3 ...};
+		}(std::make_integer_sequence<glm::length_t, Dim>());
+	}
+
+	[[nodiscard]] constexpr auto
+	area() const
+		requires(Dim == 3)
+	{
+		return glm::length(crossProduct(sideDifference(1), sideDifference(2))) / T {2};
+	}
+
+	[[nodiscard]] constexpr Normal3D
+	normal() const
+		requires(Dim == 3)
+	{
+		return crossProduct(sideDifference(1), sideDifference(2));
+	}
+
+	[[nodiscard]] constexpr Normal3D
+	nnormal() const
+		requires(Dim == 3)
+	{
+		return glm::normalize(normal());
+	}
+
+	[[nodiscard]] constexpr auto
+	sideDifference(glm::length_t side) const
+	{
+		return difference(p(side), p(0));
+	}
+
+	[[nodiscard]] constexpr auto
+	angle(glm::length_t corner) const
+	{
+		return Arc {P(corner), P(corner + 2), P(corner + 1)}.length();
+	}
+
+	template<glm::length_t D = Dim>
+	[[nodiscard]] constexpr auto
+	angleAt(const glm::vec<D, T, Q> pos) const
+		requires(D <= Dim)
+	{
+		for (glm::length_t i {}; i < 3; ++i) {
+			if (glm::vec<D, T, Q> {p(i)} == pos) {
+				return angle(i);
+			}
+		}
+		return 0.F;
+	}
+
+	[[nodiscard]] constexpr auto
+	p(const glm::length_t idx) const
+	{
+		return Base::operator[](idx);
+	}
+
+	[[nodiscard]] constexpr auto
+	P(const glm::length_t idx) const
+	{
+		return Base::operator[](idx % 3);
+	}
+
+	[[nodiscard]] constexpr Point *
+	begin()
+	{
+		return &(Base::x);
+	}
+
+	[[nodiscard]] constexpr const Point *
+	begin() const
+	{
+		return &(Base::x);
+	}
+
+	[[nodiscard]] constexpr Point *
+	end()
+	{
+		return begin() + 3;
+	}
+
+	[[nodiscard]] constexpr const Point *
+	end() const
+	{
+		return begin() + 3;
+	}
+};
diff --git a/test/test-geoData.cpp b/test/test-geoData.cpp
index 35d6bae..bd1ff87 100644
--- a/test/test-geoData.cpp
+++ b/test/test-geoData.cpp
@@ -31,18 +31,16 @@ BOOST_AUTO_TEST_CASE(loadSuccess)
 
 BOOST_AUTO_TEST_CASE(normalsAllPointUp)
 {
-	BOOST_CHECK_EQUAL(std::count_if(vertices_begin(), vertices_end(),
-							  [this](auto && vh) {
-								  return normal(vh).z > 0;
-							  }),
-			n_vertices());
+	BOOST_CHECK(std::ranges::all_of(vertices(), [this](auto && vertex) {
+		return normal(vertex).z > 0;
+	}));
 }
 
 BOOST_AUTO_TEST_CASE(trianglesContainsPoints)
 {
 	const auto face = face_handle(0);
 
-	BOOST_TEST_CONTEXT(GeoData::Triangle<2>(this, fv_range(face))) {
+	BOOST_TEST_CONTEXT(this->triangle<2>(face)) {
 		BOOST_CHECK(triangleContainsPoint(GlobalPosition2D {xllcorner, yllcorner}, face));
 		BOOST_CHECK(triangleContainsPoint(GlobalPosition2D {xllcorner + cellsize, yllcorner + cellsize}, face));
 		BOOST_CHECK(triangleContainsPoint(GlobalPosition2D {xllcorner, yllcorner + cellsize}, face));
@@ -232,7 +230,10 @@ BOOST_DATA_TEST_CASE(deform, loadFixtureJson<DeformTerrainData>("geoData/deform/
 	Surface surface;
 	surface.colorBias = RGB {0, 0, 1};
 	auto gd = std::make_shared<GeoData>(GeoData::createFlat({0, 0}, {1000000, 1000000}, 100));
-	BOOST_CHECK_NO_THROW(gd->setHeights(points, surface));
+	BOOST_CHECK_NO_THROW(gd->setHeights(points, {.surface = surface}));
+	BOOST_CHECK(std::ranges::all_of(gd->vertices(), [&gd](auto && vertex) {
+		return gd->normal(vertex).z > 0;
+	}));
 
 	ApplicationBase ab;
 	TestMainWindow tmw;
diff --git a/test/test-maths.cpp b/test/test-maths.cpp
index f7f34b3..b9d08bb 100644
--- a/test/test-maths.cpp
+++ b/test/test-maths.cpp
@@ -333,3 +333,11 @@ BOOST_DATA_TEST_CASE(rayLineDistance,
 		BOOST_CHECK_LE(Ray<RelativePosition3D>(c, direction).distanceToLine(n1, n2), 0.01F);
 	}
 }
+
+static_assert(linesIntersectAt(glm::ivec2 {10, 10}, {40, 40}, {10, 80}, {20, 40}).value().x == 24);
+static_assert(linesIntersectAt(glm::vec2 {10, 10}, {40, 40}, {10, 80}, {20, 40}).value().y == 24);
+static_assert(linesIntersectAt(GlobalPosition2D {311000100, 491100100}, {311050000, 491150000}, {312000100, 491200100},
+					  {311000100, 491100100})
+					  .value()
+		== GlobalPosition2D {311000100, 491100100});
+static_assert(!linesIntersectAt(glm::dvec2 {0, 1}, {0, 4}, {1, 8}, {1, 4}).has_value());