Voxelizer based on hexagonal grid using rational arithmetic.

hex_voxelizer.cpp

// This code turns 3D mesh into hexagonal-based voxel grid
// Most features should be preserved (algorithm is similar to Dual Contouring)
// Computations are performed on rational arithmetic to make it 100% robust

#include "vis/voxel_map.h"

#include "fwk_profile.h"
#include "geom/plane_graph.h"
#include "geom/plane_graph_builder.h"
#include "geom/visualizer.h"
#include "hash_map.h"
#include "investigator.h"
#include "rational_hex.h"
#include "vis/rational.h"

namespace vis {

template <class T>
static Rational3<T> segmentAt(const ISegment3<int> &segment, int scale, const Rational<T> &t) {
	return Rational3<T>(segment.from, scale) + Rational3<T>(segment.to - segment.from, scale) * t;
}

static int3 roundHexPos(int3 pos, int scale) {
	auto result = rationalHex(Rational3<int>(pos, scale)).hex;
	return {result.q, result.h, result.r};
}

template <class T> static int3 roundHexPos(const Rational3<T> &rpos) {
	auto result = rationalHex(rpos).hex;
	return {result.q, result.h, result.r};
}

static IBox hexBox(IBox world_box, int scale) {
	auto corners = transform(world_box.xz().corners(),
							 [](int2 pos) { return rationalToHex<false>(pos * 12); });
	auto box = enclose(corners);
	auto tmin = ratioFloor(box.min(), scale * 12);
	auto tmax = ratioCeil(box.max(), scale * 12);

	int min_y = ratioFloor(world_box.y(), scale);
	int max_y = ratioCeil(world_box.ey(), scale);
	return {tmin[0], min_y, tmin[1], tmax[0], max_y, tmax[1]};
}

// Traverses hexagonal cells
static vector<pair<int3, Rational<qint>>> segCellPoints(const ISegment3<int> &segment, int scale) {
	// Jakbyśmy to robili na intach:
	// - parametr skali, komórki mają wymiary scale * scale

	int3 hpos(roundHexPos(segment.from, scale)), hend(roundHexPos(segment.to, scale));
	int3 seg_dir = segment.to - segment.from;

	if(hpos == hend) {
		return {{hpos, Rational<qint>(1, 2)}};
	}

	// 3D Y -> W
	const int3 axes[4] = {int3(2, 0, 1), int3(0, 0, 1), int3(2, 0, -1), int3(0, 1, 0)};

	int4 dir;
	for(int i : intRange(4)) {
		auto tdot = dot(seg_dir, axes[i]);
		dir[i] = tdot < 0 ? -1 : tdot > 0 ? 1 : 0;
	}

	int2 start_pos = toRational(HexAxial<int, false>(hpos.xz())) * scale;
	int2 hex_off = segment.from.xz() - start_pos; // offset inside hex

	int4 tnum, tden;

	tden[0] = std::abs(seg_dir.z + seg_dir.x * 2);
	tden[1] = std::abs(seg_dir.z * 2);
	tden[2] = std::abs(seg_dir.z - seg_dir.x * 2);
	tden[3] = std::abs(seg_dir[1] * 8);

	tnum[0] = dir[0] > 0 ? 4 * scale - 2 * hex_off.x - hex_off.y
						 : dir[0] < 0 ? 4 * scale + 2 * hex_off.x + hex_off.y : 0;
	tnum[1] = 2 * (dir[1] ? 2 * scale - dir[1] * hex_off.y : 0);
	tnum[2] = dir[2] > 0 ? 4 * scale - 2 * hex_off.x + hex_off.y
						 : dir[2] < 0 ? 4 * scale + 2 * hex_off.x - hex_off.y : 0;
	tnum[3] = 8 * (dir[3] == -1 ? segment.from[1] - hpos.y * scale
								: (hpos.y + 1) * scale - segment.from[1]);

	int3 delta_num(scale);
	int delta = scale * 8; // deltas have same numerator, only different denominator

	llint4 t(tnum);
	for(int n : intRange(4))
		if(tden[n] == 0)
			t[n] = 1;

	Segment2<double> dseg(double2(segment.from.xz()), double2(segment.to.xz()));

	Rational<llint> prev(0, 1);
	vector<pair<int3, Rational<qint>>> out;

	llint4 offset[4] = {
		llint4(delta, delta / 2 * (dir[0] * dir[1]), delta / 2 * (dir[0] * dir[2]), 0),
		llint4(delta / 2 * (dir[0] * dir[1]), delta, delta / 2 * (-dir[2] * dir[1]), 0),
		llint4(delta / 2 * (dir[0] * dir[2]), delta / 2 * (-dir[1] * dir[2]), delta, 0),
		llint4(0, 0, 0, delta)};

	//	print("\nInput: % to %\n  Hex: % to: %\ndir: %\n", segment.from, segment.to, hpos, hend, dir);
	//	print("Num: % / %\n", tnum, tden);

	int cross_dir_xy = dir[0] > 0 || dir[2] > 0 ? 1 : 0;
	int cross_dir_xz = dir[0] > 0 ? 0 : 2;
	int cross_dir_yz = dir[2] < 0 || dir[0] < 0 ? 1 : 2;

	while(prev.num() <= prev.den()) {
		Rational<llint> rt[4] = {
			{t[0], tden[0]}, {t[1], tden[1]}, {t[2], tden[2]}, {t[3], tden[3]}};

		int order_xy = rt[0].order(rt[1]);
		int order_xz = rt[0].order(rt[2]);
		int order_yz = rt[1].order(rt[2]);

		int cross_dir = 0;

		// First three cases handle situations when ray hits hex vertex
		// This vertex belongs to only one of the hexes, that's why the strange rules
		if(order_xy == 0 && order_xz < 0)
			cross_dir = cross_dir_xy;
		else if(order_xz == 0 && order_xy < 0)
			cross_dir = cross_dir_xz;
		else if(order_yz == 0 && order_xy > 0)
			cross_dir = cross_dir_yz;
		else if(order_xy < 0 && order_xz < 0)
			cross_dir = 0;
		else if(order_xy > 0 && order_yz < 0)
			cross_dir = 1;
		else // order_xz > 0 && order_yz > 0
			cross_dir = 2;

		int order_th = rt[cross_dir].order(rt[3]);
		if(dir[3] > 0 ? order_th >= 0 : order_th > 0) {
			cross_dir = 3;
		}

		//		print("tpos:% % % % | ", (double)rt[0], (double)rt[1], (double)rt[2], (double)rt[3]);
		//		print("oxy:% oxz:% oyz:% oth:% cross_dir:% | ", order_xy, order_xz, order_yz, order_th,
		//			  cross_dir);

		out.emplace_back(make_pair(hpos, Rational<qint>(prev + rt[cross_dir]) / 2));
		prev = rt[cross_dir];
		t += offset[cross_dir];

		if(cross_dir == 0) {
			hpos[0] += dir[0];
		} else if(cross_dir == 1) {
			hpos[2] += dir[1];
		} else if(cross_dir == 2) {
			hpos[0] += dir[2];
			hpos[2] -= dir[2];
		} else {
			hpos[1] += dir[3];
		}

		//print("hpos: %\n", hpos);
		if(hpos == hend) {
			out.emplace_back(make_pair(hpos, Rational<qint>(prev + Rational<llint>(1)) / 2));
			break;
		}
	}

	for(auto &pair : out) {
		auto pos = pair.second;
		//	print("Pos: % (%)\n", pos, (double)pos);
		DASSERT(pair.second >= 0 && pair.second <= 1);
	}

	return out;
}

template <int axis> struct SegmentGenerator {
	static IBox rationalBox(IBox box, int scale) {
		int3 bmin, bmax;
		for(int i : intRange(3)) {
			// TODO: tighter bounds ?
			bmin[i] = ratioFloor(box.min(i), scale * (i == 1 ? 1 : 2));
			bmax[i] = ratioCeil(box.max(i), scale * (i == 1 ? 1 : 2));
		}
		return {bmin, bmax};
	}

	static IBox rationalWorldBox(IBox world_box, int scale) {
		auto out = rationalBox(world_box, scale);
		if(out.z() & 1)
			out = {out.min() - int3(0, 0, 1), out.max()};
		return out;
	}

	SegmentGenerator(const IBox &iworld_box, const IBox &itri_box, int scale) : m_scale(scale) {
		if(axis == 0) { // First diagonal axis
			int offset = (itri_box.z() & 1) - (itri_box.z() - iworld_box.z());
			int sx = itri_box.x() - (itri_box.size(2) + 1) / 2;
			m_rect = {sx, itri_box.y(), itri_box.ex() + 1, itri_box.ey() + 1};
			m_start = int3(offset, 0, iworld_box.z() * 2);
			m_end = m_start + int3(1, 0, 2) * iworld_box.size(2);
		} else if(axis == 1) { // Horizontal axis
			m_rect = {itri_box.z(), itri_box.y(), itri_box.ez() + 1, itri_box.ey() + 1};
			m_start = int3(iworld_box.x() * 2, 0, 0);
			m_end = int3(iworld_box.ex() * 2, 0, 0);
		} else if(axis == 2) { // Second diagonal axis
			int ox2 = (itri_box.z() & 1) + (itri_box.z() - iworld_box.z());
			int ex = itri_box.ex() + (itri_box.size(2) + 1) / 2;
			m_rect = {itri_box.x(), itri_box.y(), ex + 1, itri_box.ey() + 1};
			m_start = int3(ox2, 0, iworld_box.z() * 2);
			m_end = m_start + int3(-1, 0, 2) * iworld_box.size(2);
		} else if(axis == 3) { // Vertical axis
			m_rect = {itri_box.x(), itri_box.z(), itri_box.ex() + 1, itri_box.ez() + 1};
			m_start = int3(0, iworld_box.y(), 0);
			m_end = int3(0, iworld_box.ey(), 0);
		}

		m_start *= m_scale;
		m_end *= m_scale;
	}

	ISegment3<int> operator()(const int2 &pos) const {
		int3 off;
		if constexpr(axis == 0)
			off = int3(pos[0] * 2, pos[1], 0) * m_scale;
		else if(axis == 1)
			off = int3(pos[0] & 1, pos[1], pos[0] * 2) * m_scale;
		else if(axis == 2)
			off = int3(pos[0] * 2, pos[1], 0) * m_scale;
		else {
			off = int3(pos[0] * 2 + (pos[1] & 1), 0, pos[1] * 2);
			if(fwk::abs(off[0]) % 3 == 0) // Skipping points in the middle of hexes
				return {};
			off *= m_scale;
		}

		return {m_start + off, m_end + off};
	}

	auto begin() const { return m_rect.begin(); }
	auto end() const { return m_rect.end(); }
	auto rect() const { return m_rect; }

	void genIsects(const Triangle3<int> &tri, vector<Rational3<qint>> &out) const {
		for(auto pt : *this) {
			auto seg = (*this)(pt);
			if(auto result = isectTriangle(tri, seg))
				out.emplace_back(segmentAt(seg, m_scale, result.param()));
		}
	}

	int index(const int2 &pos) const {
		return pos.x - m_rect.x() + (pos.y - m_rect.y()) * m_rect.width();
	}

  private:
	int3 m_start, m_end;
	IRect m_rect;
	int m_scale;
};

VoxelMap voxelizeHex(CSpan<Triangle3<int>> tris, CSpan<IColor> colors, int scale) {
	FWK_PROFILE_RARE("voxelizeHex");
	VoxelMap out;

	if(tris.empty())
		return {};

	double initialization_time = getTime();
	struct ITri {
		array<geom::NodeId, 3> nodes;
		array<EdgeId, 3> edges;
		array<bool, 3> flips;
	};

	vector<ITri> itris;
	vector<SmallVector<int>> node2tri;
	vector<pair<int, int>> edge2tri;

	itris.reserve(tris.size());

	PlaneGraphBuilder<int3> builder;
	builder.reservePoints(tris.size() * 3);
	builder.reserveEdges(tris.size() * 3);
	double iscale = 1.0 / double(scale);

	for(auto tri : tris) {
		array<geom::NodeId, 3> nodes = {{builder(tri[0]), builder(tri[1]), builder(tri[2])}};
		array<bool, 3> flips = {{nodes[0] > nodes[1], nodes[1] > nodes[2], nodes[2] > nodes[0]}};
		array<EdgeId, 3> edges = {
			{flips[0] ? builder(nodes[1], nodes[0]) : builder(nodes[0], nodes[1]),
			 flips[1] ? builder(nodes[2], nodes[1]) : builder(nodes[1], nodes[2]),
			 flips[2] ? builder(nodes[0], nodes[2]) : builder(nodes[2], nodes[0])}};

		node2tri.resize(builder.numNodes());
		edge2tri.resize(builder.numEdges(), make_pair(-1, -1));
		for(auto node_id : nodes)
			node2tri[node_id].emplace_back(itris.size());
		for(auto edge_id : edges) {
			auto &ref = edge2tri[edge_id];
			if(ref.first == -1)
				ref.first = itris.size();
			else {
				//DASSERT(ref.second == -1);
				ref.second = itris.size();
			}
		}

		itris.emplace_back(ITri{nodes, edges, flips});
	}
	//	for(auto &pair : edge2tri)
	//		DASSERT(pair.first != -1 && pair.second != -1);

	ImmutableGraph tri_graph(builder.extractEdges(), builder.numNodes());
	auto points = builder.extractPoints();

	IBox wbbox = enclose(tris);
	IBox ibbox = hexBox(wbbox, scale);
	ibbox = ibbox.enlarge(3);

	vector<SimpleVoxel> voxels(ibbox.width() * ibbox.height() * ibbox.depth());
	VoxelRange<SimpleVoxel> vrange(voxels, ibbox);

	enum class FType { none, triangle, edge, vertex };
	struct Feature {
		int index = -1;
		FType type = FType::none;
		short priority = 0;

		ORDER_BY(Feature, priority, type, index);

		explicit operator bool() const { return type != FType::none; }
	};

	vector<Feature> features(voxels.size());

	initialization_time = getTime() - initialization_time;
	double isects_time = getTime();

	vector<vector<Rational3<qint>>> tri_isects_y(tris.size());

	struct TIsect {
		bool operator<(const TIsect &rhs) const { return pos < rhs.pos; }

		Rational<qint> pos;
		int tri_id;
		bool is_front;
	};

	auto iworld_box = SegmentGenerator<0>::rationalWorldBox(wbbox, scale);
	SegmentGenerator<3> vert_gen(iworld_box, iworld_box, scale);
	vector<SmallVector<TIsect, 8>> isect_map(vert_gen.rect().width() * vert_gen.rect().height());

	for(int tri_id : intRange(tris)) {
		const auto &tri = tris[tri_id];
		IBox tri_box = SegmentGenerator<0>::rationalBox(enclose(tri), scale);

		for(auto xz : tri_box.xz()) {
			// TODO: filter empty segments
			const auto &seg = vert_gen(xz);
			if(seg.empty())
				continue;

			auto index = vert_gen.index(xz);
			if(auto result = isectTriangle(tri, seg)) {
				isect_map[index].emplace_back(TIsect{result.param(), tri_id, result.is_front});
				auto rpos = segmentAt(seg, scale, result.param());
				tri_isects_y[tri_id].emplace_back(rpos);
			}
		}
	}

	for(auto xz : vert_gen) {
		auto xz_index = vert_gen.index(xz);
		auto &isects = isect_map[xz_index];
		if(isects.empty())
			continue;

		const auto &seg = vert_gen(xz);
		std::sort(begin(isects), end(isects));
		bool errors = false;

		auto rpos = rationalHex(Rational3<int>(seg.from, scale));
		int3 hex_pos(rpos.hex);
		auto point_type = rpos.classifyPoint();
		DASSERT(point_type.first == HexElement::vertex && isOneOf(point_type.second, 1, 2));
		int corner = point_type.second;

		int count = 0;
		int segment_steps = (seg.to[1] - seg.from[1]) / scale;

		for(int i : intRange(isects)) {
			TIsect &cur = isects[i];
			TIsect *next = i + 1 < isects.size() ? &isects[i + 1] : nullptr;

			if(next && cur.pos == next->pos && next->is_front == cur.is_front)
				continue;

			count += cur.is_front ? 1 : -1;
			DASSERT(!cur.pos.isInfinity());
			int start = ceil(cur.pos * segment_steps);

			int end = next ? (int)ceil(next->pos * segment_steps) : ibbox.max(1);
			if(count > 0 && !next)
				errors = true;

			if(count > 0) {
				IColor color = colors[cur.tri_id];
				IColor next_color = next ? colors[next->tri_id] : color;

				int mid = (start + end) / 2;
				auto vpos = hex_pos;
				for(int pos = start; pos < end; pos++) {
					vpos[1] = hex_pos.y + pos;
					auto col = pos >= mid ? next_color : color;
					vrange[vpos].color[corner == 2 ? 0 : 1] = col;
				}
			}
		}

		if(errors) {
			print("ERR: % - %:                 ", seg.from, seg.to);
			Rational<qint> prev;
			for(auto &isct : isects) {
				print("%(%%) ", (double)isct.pos, isct.is_front ? "+" : "-",
					  prev == isct.pos ? "*" : " ");
				prev = isct.pos;
			}
			print("\n");
		}
	}

	isects_time = getTime() - isects_time;
	double points_time = getTime();

	// TODO: better priorities for features

	for(int pid : intRange(points)) {
		vector<IColor> ncolors;
		ncolors.clear();
		for(auto tri_id : node2tri[pid])
			ncolors.emplace_back(colors[tri_id]);
		makeUnique(ncolors);

		auto pti = points[pid];
		int3 cell = roundHexPos(pti, scale);
		double3 pt = double3(pti) * iscale;
		double3 hex_pos = double3(asXZY(toRational(HexAxial<int, false>(cell.xz())), cell[1]));
		int index = vrange.index(cell);
		short priority = 100 * (colors.size() == 2 ? 6 : ncolors.size() >= 3 ? 100 : 3);
		// TODO: vertices which lie in sharp places should have higher priority
		Feature new_feature{pid, FType::vertex, priority};

		if(features[index] < new_feature) {
			voxels[index].point = float3(double3(pt) - hex_pos);
			features[index] = new_feature;
			for(auto ncol : ncolors)
				voxels[index].addFColor(ncol);
		}
	}

	points_time = getTime() - points_time;
	double edges_time = getTime();

	for(auto edge_ref : tri_graph.edgeRefs()) {
		ISegment3<int> iseg(points[edge_ref.from()], points[edge_ref.to()]);
		int src_idx = edge_ref.from(), dst_idx = edge_ref.to();

		auto ids = edge2tri[edge_ref];
		auto tdot =
			dot(Triangle3D(tris[ids.first]).normal(), Triangle3D(tris[ids.second]).normal());
		double prio_mult = 10.0 * (1.1 - fwk::abs(tdot));

		short priority = (colors[ids.first] == colors[ids.second] ? 1 : 3) * prio_mult;
		Feature new_feature{edge_ref.idx(), FType::edge, priority};

		for(auto cell : segCellPoints(iseg, scale)) {
			DASSERT_HINT(cell.second >= 0 && cell.second <= 1, cell.second);

			double3 hex_pos(
				asXZY(toRational(HexAxial<int, false>(cell.first.xz())), cell.first[1]));
			auto pt =
				lerp(double3(iseg.from), double3(iseg.to), (double)cell.second) / double(scale);
			int index = vrange.index(cell.first);

			auto &feature = features[index];
			if(feature < new_feature) {
				voxels[index].point = float3(pt - double3(hex_pos));
				feature = new_feature;
				voxels[index].addFColor(colors[ids.first]);
				voxels[index].addFColor(colors[ids.second]);
			}
		}
	}

	edges_time = getTime() - edges_time;
	double tris_time = getTime();

	HashMap<int, SmallVector<float3>> tri_points;
	vector<Rational3<qint>> tri_isects;

	for(int tri_id : intRange(tris)) {
		const auto &tri = tris[tri_id];
		const auto &itri = itris[tri_id];

		tri_points.clear();
		tri_isects.clear();
		tri_isects.insert(end(tri_isects), begin(tri_isects_y[tri_id]), end(tri_isects_y[tri_id]));

		IBox itri_box = SegmentGenerator<0>::rationalBox(enclose(tri), scale);

		SegmentGenerator<0>(iworld_box, itri_box, scale).genIsects(tri, tri_isects);
		SegmentGenerator<1>(iworld_box, itri_box, scale).genIsects(tri, tri_isects);
		SegmentGenerator<2>(iworld_box, itri_box, scale).genIsects(tri, tri_isects);

		for(auto &isect : tri_isects) {
			auto rhex = rationalHex(isect);
			HexAxial3<int, false> nbuffer[6];

			for(auto neighbour : rhex.neighbours(nbuffer)) {
				int index = vrange.index(int3(neighbour));
				if(features[index].type != FType::none)
					continue;

				auto npos = qint3(toRational(neighbour)) * isect.den();
				auto cell_point = float3(double3(isect.num() - npos) / double(isect.den()));
				tri_points[index].emplace_back(cell_point);
			}
		}

		for(auto &pair : tri_points) {
			if(pair.second.size() < 2)
				continue;

			double3 sum;
			for(auto &pt : pair.second)
				sum += double3(pt);
			auto pt = float3(sum / double(pair.second.size()));
			voxels[pair.first].point = pt;
			voxels[pair.first].addFColor(colors[tri_id]);
			features[pair.first] = Feature{tri_id, FType::triangle, 0};
		}
	}

	tris_time = getTime() - tris_time;
	double final_time = getTime();

	for(auto &voxel : voxels)
		voxel.point *= asXZY(hexRationalToWorld<float>(), 1.0f);

	out.setVoxels(vrange);
	final_time = getTime() - final_time;

	printf("Hex-Voxelizer times:\ninit:   %.02f ms\nisects: %.02f ms\npoints: %.02f ms\nedges:  "
		   "%.02f ms"
		   "\ntris:   %.02f ms\nfinal:  %.02f ms\n",
		   initialization_time * 1000.0, isects_time * 1000.0, points_time * 1000.0,
		   edges_time * 1000.0, tris_time * 1000.0, final_time * 1000.0);
	return out;

    // Testing part:
    
	HexMap<pair<IColor, IColor>, false> hmap{
		HexRect<int, false>{{ibbox.x(), ibbox.z()}, {ibbox.ex(), ibbox.ez()}},
		make_pair(IColor(ColorId::transparent), IColor(ColorId::transparent))};

	for(int x = ibbox.x(); x < ibbox.ex(); x++)
		for(int y = ibbox.y(); y < ibbox.ey(); y++)
			for(int z = ibbox.z(); z < ibbox.ez(); z++) {
				auto index = vrange.index(int3{x, y, z});
				auto col1 = vrange[index].color[0];
				auto col2 = vrange[index].color[1];
				if(col1 != ColorId::transparent) {
					HexAxial<int, false> hpos(x, z);
					hmap[hpos].first = col1;
				}
				if(col2 != ColorId::transparent) {
					HexAxial<int, false> hpos(x, z);
					hmap[hpos].second = col2;
				}
			}

	auto draw_flat_hex = [](Visualizer &vis, HMPos hex, FColor color, double scale) {
		double2 points[6];
		double2 offset = toRational(hex) * scale;
		for(int c = 0; c < 6; c++)
			points[c] = double2(rationalHexCorner<false>(c)) * scale + offset;
		for(int c = 0; c < 6; c++)
			vis.drawLine(points[c], points[(c + 1) % 6], color);
	};
	auto draw_pointy_hex = [](Visualizer &vis, HexPos hex, FColor color, double scale) {
		double2 points[6];
		double2 offset = toRational(hex) * scale;
		for(int c = 0; c < 6; c++)
			points[c] = double2(rationalHexCorner<true>(c)) * scale + offset;
		for(int c = 0; c < 6; c++)
			vis.drawLine(points[c], points[(c + 1) % 6], color);
	};

	auto vfunc = [&](geom::Visualizer &vis, double2 cursor) {
		for(int tri_id : intRange(tris)) {
			const auto &tri = tris[tri_id];
			if(tri[0].y >= 1)
				vis.drawTriangle(tri.xz(), FColor(colors[tri_id], 0.5f), false);
		}

		for(auto hpos : hmap) {
			if(distance(hpos, HMPos()) > 30)
				continue;

			auto p0 = double2(toRational(hpos)) * scale;
			auto pnq = double2(toRational(hpos + HMPos{-1, 0})) * scale;
			auto pnr = double2(toRational(hpos + HMPos{0, -1})) * scale;
			auto pqnr = double2(toRational(hpos + HMPos{1, -1})) * scale;
			auto c0 = (p0 + pnr + pnq) / 3.0;
			auto c1 = (pqnr + p0 + pnr) / 3.0;

			Triangle2D tri1(lerp(pqnr, c1, 0.1), lerp(pnr, c1, 0.1), lerp(p0, c1, 0.1));
			Triangle2D tri0(lerp(p0, c0, 0.1), lerp(pnr, c0, 0.1), lerp(pnq, c0, 0.1));
			auto col1 = FColor(hmap[hpos].first);
			auto col2 = FColor(hmap[hpos].second);

			//	if(hmap[hpos].first != ColorId::transparent)
			//		vis.drawTriangle(tri0, lerp(col1, (FColor)ColorId::black, 0.2f), true);
			//	if(hmap[hpos].second != ColorId::transparent)
			//		vis.drawTriangle(tri1, lerp(col2, (FColor)ColorId::black, 0.2f), true);

			//vis.drawPoint(c0, col1);
			//vis.drawPoint(c1, col2);

			draw_flat_hex(vis, hpos, ColorId::black, scale);
		}

		HMPos axis_pos(-60, 60);
		auto p0 = hexToWorld(axis_pos) * scale;
		auto pq = hexToWorld(axis_pos + HMPos{5, 0}) * scale;
		auto pr = hexToWorld(axis_pos + HMPos{0, 5}) * scale;

		vis.drawLine(p0, pq, ColorId::black);
		vis.drawLine(p0, pr, ColorId::black);
		vis.drawLabel(pq, "Q", FontStyle{ColorId::white});
		vis.drawLabel(pr, "R", FontStyle{ColorId::white});

		Rational2<int> cursor_rpos((int2)vround(cursor), scale);
		auto rounded = rationalHex(cursor_rpos).hex;

		array<HexAxial3<int, false>, 6> buffer;
		for(auto npos : rationalHex(cursor_rpos).neighbours(buffer))
			draw_flat_hex(vis, npos.qr(), ColorId::red, scale);
		//draw_flat_hex(vis, rounded.qr(), ColorId::yellow, scale);

		/*
		IBox bbox(asXZY(-vabs(int2(cursor)), 0), asXZY(vabs(int2(cursor)) / 2, 0));
		vis.drawRect(bbox.xz(), ColorId::black, false);

		ColorId colors[4] = {ColorId::red, ColorId::green, ColorId::blue, ColorId::white};
		for(int ax : intRange(4))
			for(auto seg : genSegments(ax, wbbox, bbox, scale)) {
				vis.drawLine(seg.from.xz(), seg.to.xz(), colors[ax]);
				vis.drawPoint(seg.from.xz(), colors[ax]);
				vis.drawPoint(seg.to.xz(), colors[ax]);
			}

		int3 from = asXZY((int2)vround(cursor), 7 * scale);
		int3 to = from / 4;
		vis.drawLine(from.xz(), to.xz(), ColorId::yellow);

		ISegment3<int> seg{from, to};
		auto out = segCellPoints(seg, scale);
		for(auto pair : out) {
			auto spos = pair.second;
			auto point = segmentAt(seg, scale, spos);
			auto dpoint = double3(segmentAt(seg, 1, spos));
			auto hpos = roundRationalHex(point.xz());
			draw_flat_hex(vis, hpos, ColorId::white, scale);
			vis.drawPoint(dpoint.xz(), ColorId::white);

			auto dpoint2 = Segment2<double>(from.xz(), to.xz()).at(double(spos));
			vis.drawCross(dpoint2, ColorId::red);
		}

		if(!isnan(sstart))
			vis.drawPoint(sstart, ColorId::black);
		if(!isnan(send))
			vis.drawPoint(send, ColorId::black);*/

		return fwk::format("Cursor: %   Pos: %  Classification: %\n", cursor_rpos, rounded,
						   rationalHex(cursor_rpos).classifyPoint());
	};

	//investigate(vfunc, none);

	return out;
}

// TODO: Errors still happen if scale is low, so this may be needed
static void perturbHexGridVertices(Span<Triangle3<int>> tris, int scale) {
	DASSERT_GT(scale, 1);

	for(auto &tri : tris) {
		int3 points[3] = {tri[0], tri[1], tri[2]};
		bool changed = false;
		for(int i : intRange(3)) {
			//	if((points[i][2] / (scale * 2)) * scale * 2 == points[i][2]) {
			//points[i][0] += 1;
			//points[i][2] += 3;
			//changed = true;
			//	}
		}
		if(changed)
			tri = {points[0], points[1], points[2]};
	}
}

VoxelMap voxelizeHex(CSpan<ColoredTriangle> tris, int density) {
	auto tc_pair = separateColors(tris);
	auto box = enclose(tc_pair.first);
	// When scale is small somehow vertices don't land where they should have (on XZ plane)
	auto scale = std::floor(bestIntegralScale({box.min(), box.max()}, 16 * 1024 * 1024));

	double3 tscale = asXZY(hexWorldToRational<double>(), 1.0) * double(scale);

	auto itris = transform(tris, [=](const Triangle3F &tri) {
		return Triangle3<int>(int3(vround(double3(tri[0]) * tscale)),
							  int3(vround(double3(tri[1]) * tscale)),
							  int3(vround(double3(tri[2]) * tscale)));
	});

	perturbHexGridVertices(itris, scale);

	return voxelizeHex(itris, tc_pair.second, scale);
}
}