CSG based on exact integer arithmetic

!csg.h

#pragma once

#include "gen_base.h"
#include "geom/immutable_graph.h"
#include "geom_base.h"

namespace geom {

// TODO: flagi (neutral jest niezależne od intersection & subtraction, addition niepotrzebne)
DEFINE_ENUM(CSGEdgeType, addition, intersection, subtraction, neutral);

template <class T> struct CSG {
	using Vec = T;
	using Segment = fwk::Segment<T>;
	using PRVec = MakeRat<Promote<T, 1>, 2>;

	using LinePt = Rational<Promote<Scalar<T>>>;

	using EType = CSGEdgeType;

	static constexpr int invalid_rvalue = INT_MIN;

	struct EdgeInfo {
		int lregion = -1, rregion = -1;
		int count = 0, label = 0;
		EType type = EType::addition;
		Segment orig_segment;
		LinePt from_t, to_t;
	};

	CSG(CSpan<Vec> points, CSpan<Pair<NodeId>> edges, CSpan<int> labels, CSpan<EType> types = {});

	void makeIntersectionGraph(CSpan<Vec>, CSpan<Pair<NodeId>>, CSpan<int>, CSpan<EType>);
	void computeRegions();
	void performSubtraction();
	void findOuterRegions();
	Expected<void> computeRegionValues();
	void computeFinalGraph();
	void investigate() const;

	int probeRegion(NodeId start_node, Vec) const;

	bool finalEdge(EdgeId) const;

	// Intersction graph
	ImmutableGraph graph;
	vector<PRVec> gpoints;
	vector<EdgeInfo> einfos;

	// Region & sub-graph information
	vector<EdgeId> subgroup_reps;
	vector<vector<EdgeId>> regions;
	vector<vector<EdgeId>> subgraphs;
	vector<vector<int>> twin_regions;
	vector<int> edge_subgraphs;
	vector<bool> removed_regions;
	vector<int> outer_regions;

	vector<int> rvalues;

	// Final result
	vector<PRVec> out_points;
	vector<Pair<NodeId>> out_edges;
	vector<bool> out_neutral_edges;
	vector<int> out_labels;
};
}

csg.cpp

#include "geom/csg.h"

#include "geom/planar_loops.h"
#include "geom/plane_graph_builder.h"
#include <fwk/math/box_iter.h>
#include <fwk/math/line.h>
#include <fwk/sys/expected.h>

#include "investigate.h"
#include "perf_base.h"
#include "vis/visualizer2.h"

#include "geom/regular_grid.h"

namespace geom {

template <class T>
CSG<T>::CSG(CSpan<Vec> input_points, CSpan<Pair<NodeId>> input_edges, CSpan<int> input_labels,
			CSpan<EType> input_types) {
	PERF_SCOPE("CSG");

	if(input_types)
		DASSERT_EQ(input_types.size(), input_edges.size());
	DASSERT_EQ(input_labels.size(), input_edges.size());

	if(!input_edges)
		return;

	makeIntersectionGraph(input_points, input_edges, input_labels, input_types);
	computeRegions();
	findOuterRegions();
	performSubtraction();
	auto result = computeRegionValues();

	if(!result) {
		result.error().print();
		investigate();
		FATAL("");
	}

	computeFinalGraph();
	//investigate();
}

template <class T>
void CSG<T>::makeIntersectionGraph(CSpan<Vec> input_points, CSpan<Pair<NodeId>> input_edges,
								   CSpan<int> input_labels, CSpan<EType> input_types) {
	PERF_SCOPE();

	PERF_CHILD_SCOPE("initialization");
	DASSERT(distinct(input_points));

	vector<LinePt> mid_points;
	vector<Segment> input_segs;

	input_segs.reserve(einfos.size());

	Box<Vec> input_box{input_points[0], input_points[0]};
	for(auto [n1, n2] : input_edges) {
		Segment seg{input_points[n1], input_points[n2]};
		input_box = enclose(input_box, enclose(seg));
		input_segs.emplace_back(seg);
	}

	int num_buckets = clamp((int)sqrt(input_edges.size()) * 4, 1, 64);

	PERF_SIBLING_SCOPE("filling buckets");

	// TODO: sweep line ?
	// TODO: this may cause inaccuracy problems; But how can we instantiate RegularGrid for Ext24?
	// TODO: simply enlarge search box for bit T's ?
	double2 enlargement = double2(input_box.size()) * 0.001;
	RegularGrid<double2> grid(DRect(input_box).enlarge(enlargement),
							  double2(input_box.size()) / num_buckets);

	vector<vector<EdgeId>> buckets(grid.width() * grid.height());
	vector<IRect> edge_cells(input_edges.size());
	auto cell_idx = [=](int2 cell) { return cell.x + cell.y * grid.width(); };

	for(auto eid : indexRange<EdgeId>(input_edges)) {
		auto [n1, n2] = input_edges[eid];
		Segment seg{input_points[n1], input_points[n2]};
		auto crect = grid.toCellRect(DRect(enclose(seg)).enlarge(enlargement));

		edge_cells[eid] = crect;
		for(auto cell : cells(crect))
			buckets[cell_idx(cell)].emplace_back(eid);
	}

	vector<int> tested(input_edges.size(), -1);

	PERF_SIBLING_SCOPE("computing intersections");

	PlaneGraphBuilder<PRVec> builder;
	// Przecięcia liczymy tylko pomiędzy źródłowymi segmentami
	// Dla każdego segmentu wyznaczamy listę punktów
	// TODO: wrzucić wszstkie midpointy do wektora (pary mid-point, indeks segmentu)
	// posortować; powinno być szybsze niż std::mapa
	for(int e1 : intRange(input_edges)) {
		auto &edge1 = input_edges[e1];
		auto &seg1 = input_segs[e1];

		mid_points = {{0}};
		tested[e1] = e1;

		PERF_SCOPE("finding mid-points");
		for(auto cell : cells(edge_cells[e1])) {
			for(int e2 : buckets[cell_idx(cell)]) {
				if(tested[e2] == e1)
					continue;
				tested[e2] = e1;

				auto edge2 = input_edges[e2];
				auto &seg2 = input_segs[e2];

				if(IsectParam ip = seg1.isectParam(seg2)) {
					if(ip.closest() != 0) {
						if(ip.closest() != 1)
							mid_points.emplace_back(ip.closest());
					}
					if(ip.isInterval())
						mid_points.emplace_back(ip.farthest());
				}
			}
		}
		PERF_CLOSE_SCOPE();

		makeSortedUnique(mid_points);
		if(mid_points.back() != 1)
			mid_points.emplace_back(1);

		EType type = input_types ? input_types[e1] : EType::addition;
		bool isect_type = isOneOf(type, EType::intersection, EType::subtraction);

		for(int i = 0; i < mid_points.size() - 1; i++) {
			int j = i + 1;
			PRVec p1(seg1.at(mid_points[i])); // TODO: downcast
			PRVec p2(seg1.at(mid_points[j]));

			// TODO: co jeśli różne typy sie pokrywają?
			// TODO: co jeśli typ neutralny wyląduje pod intersekcją ? jest to nawet możliwe
			if(auto rid = builder.find(p2, p1)) {
				if(isect_type) {
					DASSERT(einfos[*rid].type != EType::neutral);
					einfos[*rid].type =
						type == EType::subtraction ? EType::intersection : EType::subtraction;
				} else
					einfos[*rid].count--;
			} else {
				auto eid = builder(p1, p2);
				if(einfos.size() <= eid)
					einfos.resize(eid + 1);
				if(!isect_type)
					einfos[eid].count++;
				einfos[eid].label = input_labels[e1]; // TODO: multiplicity...
				einfos[eid].type = type;
				einfos[eid].orig_segment = seg1;
				einfos[eid].from_t = mid_points[i];
				einfos[eid].to_t = mid_points[j];
			}
		}
	}

	vector<Pair<NodeId>> gedges = builder.extractEdges();

	PERF_SIBLING_SCOPE("removing empty edges");
	{ // Removing unnecessary edges
		for(int n : intRange(einfos))
			if(einfos[n].count == 0 && einfos[n].type == EType::addition)
				gedges[n] = pair(NodeId(0), NodeId(0));
		auto end1 = std::remove_if(begin(einfos), end(einfos), [](auto &info) {
			return info.count == 0 && info.type == EType::addition;
		});
		auto end2 = std::remove_if(begin(gedges), end(gedges),
								   [](auto &pair) { return pair.first == pair.second; });
		einfos.erase(end1, end(einfos));
		gedges.erase(end2, end(gedges));
	}

	PERF_SIBLING_SCOPE("constructing graph");
	graph = ImmutableGraph(gedges);
	gpoints = builder.extractPoints();
	ASSERT_EQ(sortedUnique(gpoints).size(), gpoints.size());

	orderEdges<PRVec>(graph, gpoints);
	graph.computeExtendedInfo();
}

template <class T> void CSG<T>::computeRegions() {
	PERF_SCOPE();

	PERF_CHILD_SCOPE("computing edge-region mapping"); // -----------------------------------------

	regions = planarLoops<PRVec>(graph, gpoints);
	edge_subgraphs.clear();
	edge_subgraphs.resize(einfos.size(), -1);
	twin_regions.resize(regions.size());
	removed_regions.resize(regions.size(), false);

	vector<bool> flips;
	for(int r : intRange(regions)) {
		auto &loop = regions[r];

		flips.clear();

		PRVec prev_point;
		{
			auto ecur = graph.ref(loop.front()), eprev = graph.ref(loop.back());

			auto first_node =
				isOneOf(ecur.from(), eprev.from(), eprev.to()) ? ecur.from() : ecur.to();
			auto prev_node = eprev.other(first_node);

			prev_point = gpoints[first_node];
		}

		for(auto i : intRange(loop)) {
			auto eid = loop[i];
			auto p1 = gpoints[graph.from(eid)], p2 = gpoints[graph.to(eid)];
			bool flip = p1 != prev_point;
			flips.emplace_back(flip);
			prev_point = flip ? p1 : p2;
		}

		for(int i : intRange(loop)) {
			auto eid = loop[i];
			auto &rid = flips[i] ? einfos[eid].rregion : einfos[eid].lregion;
			DASSERT(rid == -1);
			rid = r;
		}
	}

	PERF_SIBLING_SCOPE("computing sub-graphs"); // ------------------------------------------------

	vector<int> stack;
	for(int e : intRange(einfos)) {
		if(edge_subgraphs[e] != -1)
			continue;
		int sgid = subgraphs.size();
		auto &sg = subgraphs.emplace_back();

		stack.clear();
		stack.emplace_back(e);

		while(!stack.empty()) {
			auto eid = stack.back();
			stack.pop_back();

			if(edge_subgraphs[eid] != -1)
				continue;

			sg.emplace_back(eid);
			edge_subgraphs[eid] = sgid;

			for(int nid : regions[einfos[eid].lregion])
				if(edge_subgraphs[nid] == -1)
					stack.emplace_back(nid);
			for(int nid : regions[einfos[eid].rregion])
				if(edge_subgraphs[nid] == -1)
					stack.emplace_back(nid);
		}
	}

	PERF_SIBLING_SCOPE("Finding twin-regions"); // ------------------------------------------------

	for(int sg : intRange(subgraphs)) {
		auto &sub_graph = subgraphs[sg];

		Maybe<EdgeId> best_edge;
		using PPRT = MakeRat<Promote<T, 2>, 0>;
		PPRT best_pos = inf;

		for(auto eid : sub_graph) {
			auto &info = einfos[eid];
			auto seg = info.orig_segment;
			auto n1 = graph.from(eid), n2 = graph.to(eid);

			PPRT pos = min(gpoints[n1].x(), gpoints[n2].x());
			// TODO: handle cases where there are no such segments
			if((pos < best_pos || !best_edge) && seg.dir().x < 0) {
				best_edge = eid;
				best_pos = pos;
			}
		}

		if(!best_edge)
			continue; // TODO: handle that
		subgroup_reps.emplace_back(*best_edge);
		//print("[%] Best seg: [%] %\n", sg, *best_edge, best_seg);

		auto &info = einfos[*best_edge];
		auto best_seg = info.orig_segment;
		auto start_pt = info.to_t;
		Line line{best_seg.from, best_seg.dir()};

		int best_region = probeRegion(graph.to(*best_edge), line.dir);

		Pair<LinePt, int> min_pt = {inf, {}};

		for(auto e : intRange(einfos)) {
			auto &seg = einfos[e].orig_segment;
			Line rline{seg.from, seg.dir()};

			auto iparam1 = line.isectParam(rline);
			auto iparam2 = rline.isectParam(line);

			if(iparam1.isPoint() && iparam1.closest() > start_pt &&
			   iparam2.closest() >= einfos[e].from_t && iparam2.closest() <= einfos[e].to_t)
				min_pt = min(min_pt, {iparam1.closest(), e});
		}

		if(min_pt.first != inf) {
			auto eref = graph.ref(EdgeId(min_pt.second));
			auto hit_pos = best_seg.at(min_pt.first);

			Maybe<NodeId> node;
			if(hit_pos == gpoints[eref.from()])
				node = eref.from();
			else if(hit_pos == gpoints[eref.to()])
				node = eref.to();

			int hit_region;
			if(node) {
				hit_region = probeRegion(*node, -line.dir);
				//print("Hit node: % (region: %)\n", hit_pos, hit_region);
			} else {
				auto hit_seg = einfos[eref].orig_segment;
				bool hit_left_side = ccwSide(hit_seg.dir(), -best_seg.dir());
				hit_region = hit_left_side ? einfos[eref].lregion : einfos[eref].rregion;
			}

			//print("Joining sub_graphs: % - % (regions: % - %)\n", sg, edge_subgraphs[min_pt.second],
			//	  hit_region, best_region);

			if(hit_region != best_region) {
				twin_regions[best_region].emplace_back(hit_region);
				twin_regions[hit_region].emplace_back(best_region);
			}
		}
	}

	for(auto &twins : twin_regions)
		makeSortedUnique(twins);
}

template <class T> void CSG<T>::findOuterRegions() {
	outer_regions.clear();

	vector<NodeId> ordered_nodes;
	ordered_nodes.reserve(graph.numNodes());

	for(auto nid : graph.nodeIds())
		if(graph.numEdges(nid) >= 1)
			ordered_nodes.emplace_back(nid);
	std::sort(begin(ordered_nodes), end(ordered_nodes),
			  [&](int n1, int n2) { return gpoints[n1][0] < gpoints[n2][0]; });

	vector<bool> visited(regions.size(), false);
	vector<int> stack;
	for(auto node : ordered_nodes) {
		int r = probeRegion(node, Vec(-1, 0));
		if(visited[r])
			continue;

		stack = {r};
		outer_regions.emplace_back(r);
		for(auto twin : twin_regions[r])
			outer_regions.emplace_back(twin);

		while(stack) {
			int r = stack.back();
			stack.pop_back();

			if(visited[r])
				continue;
			visited[r] = true;

			for(auto twin : twin_regions[r])
				if(!visited[twin])
					stack.emplace_back(twin);

			for(auto e : regions[r]) {
				auto &info = einfos[e];
				int other = info.lregion == r ? info.rregion : info.lregion;
				if(!visited[other])
					stack.emplace_back(other);
			}
		}
	}

	makeSorted(outer_regions);

	for(auto r1 : outer_regions)
		for(auto r2 : outer_regions)
			if(r1 != r2)
				twin_regions[r1].emplace_back(r2);
	for(auto &twins : twin_regions)
		makeSortedUnique(twins);
}

template <class T> void CSG<T>::performSubtraction() {
	vector<int> stack;

	for(auto estart : graph.edgeRefs()) {
		auto &info = einfos[estart];
		if(isOneOf(info.type, EType::intersection, EType::subtraction))
			stack.emplace_back(info.type == EType::intersection ? info.rregion : info.lregion);
	}

	vector<bool> visited(regions.size(), false);
	while(stack) {
		int r = stack.back();
		stack.pop_back();

		if(visited[r])
			continue;
		visited[r] = true;
		removed_regions[r] = true;
		//print("Remove: %\n", r);

		for(auto twin : twin_regions[r])
			if(!visited[twin])
				stack.emplace_back(twin);

		for(auto edge : regions[r]) {
			auto &info = einfos[edge];
			if(!isOneOf(info.type, EType::intersection, EType::subtraction)) {
				int other = info.lregion == r ? info.rregion : info.lregion;
				if(!visited[other])
					stack.emplace_back(other);
			}
		}
	}
}

template <class T> Expected<void> CSG<T>::computeRegionValues() {
	PERF_SCOPE();

	rvalues.clear();
	rvalues.resize(regions.size(), invalid_rvalue);

	Error out_error;
	bool rvalue_fail = false;

	vector<Pair<int>> stack;
	for(auto rstart : outer_regions) {
		if(rvalues[rstart] != invalid_rvalue)
			continue;

		stack.emplace_back(rstart, 0);
		for(auto twin : twin_regions[rstart])
			stack.emplace_back(twin, 0);

		while(stack) {
			auto [rid, value] = stack.back();
			stack.pop_back();

			if(!isOneOf(rvalues[rid], invalid_rvalue, value) && !rvalue_fail) {
				out_error += format("Inconsistent rvalues: region:%: value:% -> %\n", rid,
									rvalues[rid], value);
				rvalue_fail = true;
			}

			if(rvalues[rid] != invalid_rvalue || rvalues[rid] == value)
				continue;
			rvalues[rid] = value;
			//print("[%] = %\n", rid, value);

			for(auto eid : regions[rid]) {
				auto &edge = einfos[eid];
				auto [nrid, nvalue] = edge.lregion == rid ? pair(edge.rregion, value + edge.count)
														  : pair(edge.lregion, value - edge.count);

				if(!isOneOf(rvalues[nrid], invalid_rvalue, nvalue) && !rvalue_fail) {
					out_error += format("Inconsistent rvalues: region:% value:% -> % from:%\n",
										nrid, rvalues[nrid], nvalue, rid);
					rvalue_fail = true;
				}

				if(rvalues[nrid] == invalid_rvalue) {
					stack.emplace_back(nrid, nvalue);
					for(auto twin : twin_regions[nrid])
						stack.emplace_back(twin, nvalue);
				}
			}
		}
	}

	for(auto r : intRange(regions))
		if(removed_regions[r])
			rvalues[r] = 0;

	if(rvalue_fail)
		return out_error;
	return {};
}

template <class T> bool CSG<T>::finalEdge(EdgeId eid) const {
	auto &info = einfos[eid];
	auto lvalue = rvalues[info.lregion], rvalue = rvalues[info.rregion];
	return (lvalue != 0) != (rvalue != 0) || (info.type == EType::neutral && lvalue != 0);
}

template <class T> void CSG<T>::investigate() const {
	using namespace vis;
	auto vis_func = [&](Visualizer2 &vis, double2 mouse_pos) -> string {
		auto pts = transform<double2>(gpoints);

		vis(pts);
		for(auto eref : graph.edgeRefs()) {
			bool is_representative = isOneOf(eref.id(), subgroup_reps);
			auto col = is_representative ? ColorId::yellow
										 : finalEdge(eref) ? ColorId::white : ColorId::gray;
			vis(Segment2D{pts[eref.from()], pts[eref.to()]}, {col, VisOpt::arrow});
		}

		TextFormatter fmt;

		if(1) { // Showing subgraphs
			pair<double, int> closest = {inf, 0};

			for(int n : intRange(subgraphs)) {
				double2 center;
				for(auto eid : subgraphs[n])
					center += pts[graph.from(eid)];
				center /= subgraphs[n].size();
				closest = min(closest, pair(distance(center, mouse_pos), n));
			}
			for(auto eid : subgraphs[closest.second]) {
				auto col = finalEdge(eid) ? ColorId::red : IColor(200, 100, 100);
				vis(Segment2D{pts[graph.from(eid)], pts[graph.to(eid)]}, {col, VisOpt::arrow});
			}
			fmt("Sub-graph: %\n", closest.second);
		}

		if(1) { // Showing edges
			pair<double, int> closest = {inf, 0};

			for(auto eref : graph.edgeRefs()) {
				fwk::Segment seg{pts[eref.from()], pts[eref.to()]};
				closest = min(closest, {seg.distanceSq(mouse_pos), eref.idx()});
			}

			EdgeId eid(closest.second);
			vis(Segment2D{pts[graph.from(eid)], pts[graph.to(eid)]},
				{ColorId::cyan, VisOpt::arrow});
			auto lr = einfos[eid].lregion, rr = einfos[eid].rregion;
			fmt("%: Left:%[%%] Right:%[%%] count:% type:%\nFrom:% To:% (Orig: % % - %)", eid, lr,
				rvalues[lr], removed_regions[lr] ? " removed" : "", rr, rvalues[rr],
				removed_regions[rr] ? " removed" : "", einfos[eid].count, einfos[eid].type,
				FormatMode::structured, gpoints[graph.from(eid)], gpoints[graph.to(eid)],
				einfos[eid].orig_segment, einfos[eid].from_t, einfos[eid].to_t);
		}

		return fmt.text();
	};

	::investigate(vis_func, none, none);
}

template <class T> void CSG<T>::computeFinalGraph() {
	PERF_SCOPE();

	PlaneGraphBuilder<PRVec> builder;
	out_labels.resize(einfos.size() * 2, -1);
	out_neutral_edges.resize(einfos.size() * 2, false);

	// TODO: describe why neutral edges are handled differently...
	for(auto eref : graph.edgeRefs()) {
		if(finalEdge(eref)) {
			auto &info = einfos[eref];
			auto lvalue = rvalues[info.lregion], rvalue = rvalues[info.rregion];
			auto p1 = gpoints[eref.from()], p2 = gpoints[eref.to()];
			if(info.type == EType::neutral) {
				auto eid1 = builder(p1, p2);
				auto eid2 = builder(p2, p1);
				out_labels[eid1] = info.label;
				out_labels[eid2] = info.label;
				out_neutral_edges[eid1] = out_neutral_edges[eid2] = true;
			} else {
				auto eid = lvalue != 0 ? builder(p2, p1) : builder(p1, p2);
				out_labels[eid] = info.label;
			}
		}
	}

	out_points = builder.extractPoints();
	out_edges = builder.extractEdges();
	out_labels.resize(out_edges.size());
	out_neutral_edges.resize(out_edges.size());

	ASSERT_EQ(sortedUnique(transform<float2>(out_points)).size(), out_points.size());
	ASSERT_EQ(sortedUnique(out_edges).size(), out_edges.size());
}

template <class T> int CSG<T>::probeRegion(NodeId node_id, Vec dir) const {
	auto node = graph.ref(node_id);

	CSpan<EdgeId> edges = graph.edges(node);

	int ccw_edge_idx = ccwNext<Vec>(dir, edges.size(), [&](int idx) {
		auto eref = graph.ref(edges[idx]);
		auto dir = einfos[eref].orig_segment.dir();
		return eref.from() == node ? dir : -dir;
	});

	auto eref = graph.ref(edges[ccw_edge_idx]);
	return eref.from() == node ? einfos[eref].rregion : einfos[eref].lregion;
}

template struct CSG<vec2<int>>;
}