envi-code/SourceCode/Code2026/cad_mcp_plugins/bvh.h

#pragma once
// ============================================================================
//  bvh.h
//  2D AABB 层次包围盒 —— 干涉检查的**粗筛**层。
//
//  纯 C++（不依赖 ObjectARX），header-only，便于复用与推理。
//
//  相比「按质心中位分割」的朴素实现，这里的取舍：
//    - 按**质心中位数**沿**较长轴**二分 → 深度严格 O(log n)，无退化分支；
//    - 内部节点记录**子树图元数**，交叉遍历时优先拆较大的一侧，减少无效下降；
//    - 自相交遍历**每对只报告一次**（叶内 i<j + 子树两两交叉），无重复无自比。
//
//  坐标约定：x 向右、y 向上；Box 为 (x1,y1)-(x2,y2)。
// ============================================================================

#include <algorithm>
#include <utility>
#include <vector>

namespace dwg_bvh
{

// 叶节点最多容纳的图元数：越小查询越快、建树越慢
constexpr int kLeafSize = 4;

struct Box
{
	double x1 = 0.0, y1 = 0.0, x2 = 0.0, y2 = 0.0;

	double width()  const { return x2 - x1; }
	double height() const { return y2 - y1; }
	double area()   const { return width() * height(); }
	bool   valid()  const { return x2 >= x1 && y2 >= y1; }

	// 反向极值盒：任何 grow 都能把它撑开（用于求并集）
	static Box empty()
	{
		Box b;
		b.x1 = b.y1 = 1e300;
		b.x2 = b.y2 = -1e300;
		return b;
	}

	void grow(const Box& o)
	{
		if (o.x1 < x1) x1 = o.x1;
		if (o.y1 < y1) y1 = o.y1;
		if (o.x2 > x2) x2 = o.x2;
		if (o.y2 > y2) y2 = o.y2;
	}

	// 接触即算相交
	bool intersects(const Box& o) const
	{
		return !(x2 < o.x1 || o.x2 < x1 || y2 < o.y1 || o.y2 < y1);
	}

	// 交集盒（可能退化，调用方自行判断）
	Box overlap(const Box& o) const
	{
		Box r;
		r.x1 = (x1 > o.x1) ? x1 : o.x1;
		r.y1 = (y1 > o.y1) ? y1 : o.y1;
		r.x2 = (x2 < o.x2) ? x2 : o.x2;
		r.y2 = (y2 < o.y2) ? y2 : o.y2;
		return r;
	}
};

struct Primitive
{
	Box box;
	int payload = -1;   // 调用方的索引（如实体在自身数组里的下标）
};

struct Node
{
	Box  box;
	int  first = 0;     // 叶：图元区间起点；内部：左子节点下标
	int  count = 0;     // 叶：图元个数；内部：子树图元总数
	bool leaf  = true;
};

class Tree
{
public:
	// 建树（就地重排 prims；payload 随图元一起搬，调用方索引不受影响）
	void Build(std::vector<Primitive> prims)
	{
		m_prims.swap(prims);
		m_nodes.clear();
		if (m_prims.empty()) return;
		m_nodes.reserve(2 * m_prims.size());
		m_nodes.emplace_back();
		buildNode(0, 0, static_cast<int>(m_prims.size()));
	}

	bool empty() const { return m_nodes.empty(); }
	int  size()  const { return static_cast<int>(m_prims.size()); }

	// 两两自检：所有包围盒相交的 (payloadA, payloadB)，每对仅一次
	void SelfPairs(std::vector<std::pair<int, int>>& out) const
	{
		if (!m_nodes.empty()) selfPairs(0, out);
	}

	// 查询与 box 相交的 payload
	void Query(const Box& q, std::vector<int>& out) const
	{
		out.clear();
		if (m_nodes.empty()) return;
		std::vector<int> stack;
		stack.push_back(0);
		while (!stack.empty())
		{
			const int idx = stack.back();
			stack.pop_back();
			const Node& n = m_nodes[idx];
			if (!n.box.intersects(q)) continue;
			if (n.leaf)
			{
				const int end = n.first + n.count;
				for (int i = n.first; i < end; ++i)
					if (m_prims[i].box.intersects(q)) out.push_back(m_prims[i].payload);
			}
			else
			{
				stack.push_back(n.first);
				stack.push_back(n.first + 1);
			}
		}
	}

private:
	void buildNode(int nodeIdx, int first, int count)
	{
		Box hull = Box::empty();
		Box cent = Box::empty();
		for (int i = first; i < first + count; ++i)
		{
			const Box& b = m_prims[i].box;
			hull.grow(b);
			Box c;
			c.x1 = c.x2 = (b.x1 + b.x2) * 0.5;
			c.y1 = c.y2 = (b.y1 + b.y2) * 0.5;
			cent.grow(c);
		}

		m_nodes[nodeIdx].box   = hull;
		m_nodes[nodeIdx].first = first;
		m_nodes[nodeIdx].count = count;
		m_nodes[nodeIdx].leaf  = true;
		if (count <= kLeafSize) return;

		const bool byX = cent.width() >= cent.height();
		const int  mid = count / 2;
		auto begin = m_prims.begin() + first;
		std::nth_element(begin, begin + mid, begin + count,
			[byX](const Primitive& a, const Primitive& b)
			{
				const double ca = byX ? (a.box.x1 + a.box.x2) : (a.box.y1 + a.box.y2);
				const double cb = byX ? (b.box.x1 + b.box.x2) : (b.box.y1 + b.box.y2);
				return ca < cb;
			});

		// 注意：emplace_back 可能重分配 m_nodes，故不持有 Node& 引用
		const int leftIdx = static_cast<int>(m_nodes.size());
		m_nodes.emplace_back();
		m_nodes.emplace_back();
		m_nodes[nodeIdx].first = leftIdx;
		m_nodes[nodeIdx].leaf  = false;

		buildNode(leftIdx, first, mid);
		buildNode(leftIdx + 1, first + mid, count - mid);
	}

	void selfPairs(int idx, std::vector<std::pair<int, int>>& out) const
	{
		const Node& n = m_nodes[idx];
		if (n.leaf)
		{
			const int end = n.first + n.count;
			for (int i = n.first; i < end; ++i)
				for (int j = i + 1; j < end; ++j)
					if (m_prims[i].box.intersects(m_prims[j].box))
						out.emplace_back(m_prims[i].payload, m_prims[j].payload);
			return;
		}
		selfPairs(n.first, out);
		selfPairs(n.first + 1, out);
		crossPairs(n.first, n.first + 1, out);
	}

	void crossPairs(int a, int b, std::vector<std::pair<int, int>>& out) const
	{
		const Node& na = m_nodes[a];
		const Node& nb = m_nodes[b];
		if (!na.box.intersects(nb.box)) return;

		if (na.leaf && nb.leaf)
		{
			const int ea = na.first + na.count;
			const int eb = nb.first + nb.count;
			for (int i = na.first; i < ea; ++i)
				for (int j = nb.first; j < eb; ++j)
					if (m_prims[i].box.intersects(m_prims[j].box))
						out.emplace_back(m_prims[i].payload, m_prims[j].payload);
			return;
		}

		// 优先拆「较大」的一侧，让递归更快收敛
		const bool splitA = nb.leaf || (!na.leaf && na.count >= nb.count);
		if (splitA)
		{
			crossPairs(na.first, b, out);
			crossPairs(na.first + 1, b, out);
		}
		else
		{
			crossPairs(a, nb.first, out);
			crossPairs(a, nb.first + 1, out);
		}
	}

	std::vector<Primitive> m_prims;
	std::vector<Node>      m_nodes;
};

} // namespace dwg_bvh