kev/Drawer/GVision/SurfaceGrid/lib/QuadTree.h

#pragma once
#include <iostream>
#include <vector>
#include <algorithm>
using namespace std;
struct PointT {
	double x, y;
};

struct LineSegment {
	PointT start, end;
};

class QuadTreeNode {
public:
	QuadTreeNode(double x, double y, double width, double height, int capacity)
		: boundary{ x, y, width, height }, capacity(capacity), divided(false) {}

	bool insert(const LineSegment& segment) {
		if (!intersects(segment, boundary)) {
			return false;
		}

		if (segments.size() < capacity) {
			segments.push_back(segment);
			return true;
		}

		if (!divided) {
			subdivide();
		}

		if (northeast->insert(segment)) return true;
		if (northwest->insert(segment)) return true;
		if (southeast->insert(segment)) return true;
		if (southwest->insert(segment)) return true;

		return false;
	}

	bool hasIntersection(const LineSegment& querySegment) {
		if (!intersects(querySegment, boundary)) {
			return false;
		}

		for (const auto& segment : segments) {
			if (segmentsIntersect(querySegment, segment)) {
				return true;
			}
		}

		if (divided) {
			if (northeast->hasIntersection(querySegment)) return true;
			if (northwest->hasIntersection(querySegment)) return true;
			if (southeast->hasIntersection(querySegment)) return true;
			if (southwest->hasIntersection(querySegment)) return true;
		}

		return false;
	}

private:
	struct Boundary {
		double x, y, width, height;
	} boundary;

	int capacity;
	std::vector<LineSegment> segments;
	bool divided;
	QuadTreeNode* northeast;
	QuadTreeNode* northwest;
	QuadTreeNode* southeast;
	QuadTreeNode* southwest;

	void subdivide() {
		double x = boundary.x;
		double y = boundary.y;
		double w = boundary.width / 2;
		double h = boundary.height / 2;

		northeast = new QuadTreeNode(x + w, y, w, h, capacity);
		northwest = new QuadTreeNode(x, y, w, h, capacity);
		southeast = new QuadTreeNode(x + w, y + h, w, h, capacity);
		southwest = new QuadTreeNode(x, y + h, w, h, capacity);
		divided = true;
	}

	bool intersects(const LineSegment& segment, const Boundary& boundary) {
		double minX = segment.start.x;
		double maxX = segment.end.x;
		if (maxX < minX) {
			minX = segment.end.x;
			maxX = segment.start.x;
		}
		double minY = segment.start.y;
		double maxY = segment.end.y;
		if (maxY < minY)
		{
			minY = segment.end.y;
			maxY = segment.start.y;
		}

		return !(minX > boundary.x + boundary.width || maxX < boundary.x ||
			minY > boundary.y + boundary.height || maxY < boundary.y);
	}

	bool segmentsIntersect(const LineSegment& seg1, const LineSegment& seg2) {
		auto orientation = [](const PointT& p, const PointT& q, const PointT& r) {
			double val = (q.y - p.y) * (r.x - q.x) - (q.x - p.x) * (r.y - q.y);
			if ((std::abs)(val) < 1.0E-5) return 0; // collinear
			return (val > 0.0) ? 1 : 2; // clock or counterclock wise
		};

		auto onSegment = [](const PointT& p, const PointT& q, const PointT& r) {
			return q.x <= (std::max)(p.x, r.x) && q.x >= (std::min)(p.x, r.x) &&
				q.y <= (std::max)(p.y, r.y) && q.y >= (std::min)(p.y, r.y);
		};

		PointT p1 = seg1.start, q1 = seg1.end;
		PointT p2 = seg2.start, q2 = seg2.end;

		int o1 = orientation(p1, q1, p2);
		int o2 = orientation(p1, q1, q2);
		int o3 = orientation(p2, q2, p1);
		int o4 = orientation(p2, q2, q1);

		if (o1 != o2 && o3 != o4)
			return true;

		if (o1 == 0 && onSegment(p1, p2, q1)) return true;
		if (o2 == 0 && onSegment(p1, q2, q1)) return true;
		if (o3 == 0 && onSegment(p2, p1, q2)) return true;
		if (o4 == 0 && onSegment(p2, q1, q2)) return true;

		return false;
	}
};

class CQuadTree
{
public:
	CQuadTree(double x, double y, double width, double height, int capacity)
		: root(new QuadTreeNode(x, y, width, height, capacity)) {}

	void insert(const LineSegment& segment) {
		root->insert(segment);
	}

	//void remove(const LineSegment& segment) {
	//	root->remove(segment);
	//}

	bool hasIntersection(const LineSegment& querySegment) {
		return root->hasIntersection(querySegment);
	}

private:
	QuadTreeNode* root;
};

//bool operator==(const LineSegment& lhs, const LineSegment& rhs) {
//	return lhs.start.x == rhs.start.x && lhs.start.y == rhs.start.y &&
//		lhs.end.x == rhs.end.x && lhs.end.y == rhs.end.y;
//}