public KD() { g_items = new List <SortItem>(); m_hierarchy = new TempNode(); g_totalPolys = 0; g_maxDepth = 0; g_numNodes = 0; m_aabb = new AABB(); m_aabb.m_mn = new Vector3(0, 0, 0); m_aabb.m_mx = new Vector3(0, 0, 0); }
public int getDepth(TempNode n) { g_numNodes++; if (n.m_splitAxis < 0) { return(1); } int d0 = getDepth(n.m_children[0]); int d1 = getDepth(n.m_children[1]); if (d0 > d1) { return(d0 + 1); } return(d1 + 1); }
// 创建 KD 树,得到 KD 树相关信息 public void constructHierarchy(ref List <Polygon> polygons, int numPolygons) { g_totalPolys = 0; // 计算包围盒,构建排序数组 g_items = new List <SortItem>(); for (int i = 0; i < 2 * numPolygons; i++) { g_items.Add(new SortItem()); } for (int i = 0; i < 3; i++) { m_aabb.m_mn[i] = m_aabb.m_mx[i] = polygons[0][0][i]; } for (int i = 0; i < numPolygons; i++) { Polygon poly = polygons[i]; for (int j = 0; j < poly.numPoints(); j++) { m_aabb.grow(poly[j]); } } // 稍微扩大包围盒,把多边形的边界也包含进来 Vector3 v = new Vector3(EPS_BOUNDING_BOX, EPS_BOUNDING_BOX, EPS_BOUNDING_BOX); m_aabb.m_mn -= v; m_aabb.m_mx += v; // 构建 KD 树 m_hierarchy = constructRecursive(ref polygons, numPolygons, ref m_aabb); // 根节点 Console.WriteLine("场景中的障碍物面总数:{0}", g_totalPolys); // 计算最大深度 g_numNodes = 0; g_maxDepth = getDepth(m_hierarchy); // KD 树的最大深度 Console.WriteLine("KD 树节点数: {0}", g_numNodes); Console.WriteLine("KD 树深度: {0}", g_maxDepth); // 清空 g_items.Clear(); }
public void beamCastRecursive(TempNode node) { // beam 不会进入当前区域 if (g_beamBeam.numPleqs() > 0 && !intersectAABBFrustum(ref g_beamMid, ref g_beamDiag, ref g_beamBeam)) { return; } // 叶子 if (node.m_splitAxis < 0) { for (int i = 0; i < node.m_numPolygons; i++) { Polygon poly = node.m_polygons[i]; if (g_foundPolygons.Contains(poly.m_id)) // 已经保存过了 { continue; } g_beamResult.Add(new Polygon(ref poly)); g_foundPolygons.Add(poly.m_id); } return; } // 递归 int axis = node.m_splitAxis; float splitPos = node.m_splitPos; float om = g_beamMid[axis]; float od = g_beamDiag[axis]; g_beamMid[axis] = (float)0.5 * (om - od + splitPos); g_beamDiag[axis] = splitPos - g_beamMid[axis]; beamCastRecursive(node.m_children[0]); g_beamMid[axis] = (float)0.5 * (om + od + splitPos); g_beamDiag[axis] = g_beamMid[axis] - splitPos; beamCastRecursive(node.m_children[1]); g_beamMid[axis] = om; g_beamDiag[axis] = od; }
public Polygon rayCastList(TempNode node, float dEnter, float dExit) { if (dEnter < 0) { dEnter = 0; } if (dExit > 1) { dExit = 1; } if (dEnter > dExit + EPS_DISTANCE) { return(null); } if (node.m_splitAxis < 0) { // 叶子节点 if (node.m_numPolygons > 0) { Polygon poly = isectPolygons(ref node.m_polygons, node.m_numPolygons, dEnter, dEnter); if (poly != null) { return(poly); } } return(null); } float d = getSplitDistance(node.m_splitPos, node.m_splitAxis); if (g_dirsgn[node.m_splitAxis] == 0) // 正数 { if (node.m_children[1] != null && d <= dExit + EPS_DISTANCE) { float newEnter = dEnter; if (d > newEnter) { newEnter = d; } rayCastList(node.m_children[1], newEnter, dExit); } if (node.m_children[0] != null && d >= dEnter - EPS_DISTANCE) { if (d < dExit) { dExit = d; } rayCastList(node.m_children[0], dEnter, dExit); } } else { if (node.m_children[0] != null && d <= dExit + EPS_DISTANCE) { float newEnter = dEnter; if (d > newEnter) { newEnter = d; } rayCastList(node.m_children[0], newEnter, dExit); } if (node.m_children[1] != null && d >= dEnter - EPS_DISTANCE) { if (d < dExit) { dExit = d; } rayCastList(node.m_children[1], dEnter, dExit); } } return(null); }
public bool rayCastListAny(TempNode node, float dEnter, float dExit, int polygonId, int prePolyId) { if (dEnter < 0) { dEnter = 0; } if (dExit > 1) { dExit = 1; } if (dEnter > dExit + EPS_DISTANCE) { return(false); } if (node.m_splitAxis < 0) { // 叶子节点 if (node.m_numPolygons > 0) { if (isectPolygonsAny(ref node.m_polygons, node.m_numPolygons, polygonId, prePolyId)) { return(true); } } return(false); } float d = getSplitDistance(node.m_splitPos, node.m_splitAxis); if (g_dirsgn[node.m_splitAxis] == 0) // 正数 { if (node.m_children[1] != null && d <= dExit + EPS_DISTANCE) { float newEnter = dEnter; if (d > newEnter) { newEnter = d; } if (rayCastListAny(node.m_children[1], newEnter, dExit, polygonId, prePolyId)) { return(true); } } if (node.m_children[0] != null && d >= dEnter - EPS_DISTANCE) { if (d < dExit) { dExit = d; } if (rayCastListAny(node.m_children[0], dEnter, dExit, polygonId, prePolyId)) { return(true); } } } else { if (node.m_children[0] != null && d <= dExit + EPS_DISTANCE) { float newEnter = dEnter; if (d > newEnter) { newEnter = d; } if (rayCastListAny(node.m_children[0], newEnter, dExit, polygonId, prePolyId)) { return(true); } } if (node.m_children[1] != null && d >= dEnter - EPS_DISTANCE) { if (d < dExit) { dExit = d; } if (rayCastListAny(node.m_children[1], dEnter, dExit, polygonId, prePolyId)) { return(true); } } } return(false); }
// 每个节点保存多边形数组,多边形数量,分割面,子节点指针 public TempNode constructRecursive(ref List <Polygon> polygons, int numPolygons, ref AABB aabb) { // 叶子 if (numPolygons <= g_maxPolygonsInLeaf) { g_totalPolys += numPolygons; TempNode n = new TempNode(); n.m_numPolygons = numPolygons; if (numPolygons > 0) { n.m_polygons = new List <Polygon>(); for (int i = 0; i < numPolygons; i++) { n.m_polygons.Add(new Polygon(polygons[i])); } } return(n); } // 寻找最佳分割面 float splitPos = 0; int axis = getOptimalSplitPlane(ref polygons, numPolygons, ref splitPos, ref aabb); if (axis < 0) // 不可分割,成为叶子 { g_totalPolys += numPolygons; TempNode n = new TempNode(); n.m_numPolygons = numPolygons; n.m_polygons = new List <Polygon>(); for (int i = 0; i < polygons.Count; i++) { n.m_polygons.Add(new Polygon(polygons[i])); } return(n); } // 分割 TempNode n1 = new TempNode(); n1.m_splitAxis = axis; n1.m_splitPos = splitPos; n1.m_numPolygons = numPolygons; // 将多边形分类 for (int c = 0; c < 2; c++) // 0,分割面左侧;1,分割面右侧 { AABB aabb2 = new AABB(ref aabb); // 父区域大小 if (c == 0) { aabb2.m_mx[axis] = splitPos; } else { aabb2.m_mn[axis] = splitPos; } AABB aabbTest = new AABB(ref aabb2); // 子区域大小 // 每个维度扩大一些 Vector3 v = new Vector3(EPS_POLY_BOX_OVERLAP, EPS_POLY_BOX_OVERLAP, EPS_POLY_BOX_OVERLAP); aabbTest.m_mn -= v; aabbTest.m_mx += v; // 每个多边形应该位于哪个子区域内 int childPolys = 0; for (int i = 0; i < numPolygons; i++) { Polygon poly = new Polygon(polygons[i]); // 复制 AABB pbox = poly.getAABB(); bool overlap = false; if (pbox.m_mn[axis] == splitPos && pbox.m_mx[axis] == splitPos) // 本身是一个分割面 { overlap = (c == 1); // 本身是分割面,属于右子节点 } else { // 确定分割面 for (int j = 0; j < poly.numPoints(); j++) { float x = poly[j][axis]; if (c == 0 && x < splitPos) { overlap = true; // 被包含在当前半区域内 } if (c == 1 && x > splitPos) { overlap = true; } } } if (!overlap) { continue; } if (poly.clip(ref aabbTest) != Polygon.ClipResult.CLIP_VANISHED) { if (i != childPolys) { swap(polygons[i], polygons[childPolys]); } childPolys++; } } n1.m_children[c] = constructRecursive(ref polygons, childPolys, ref aabb2); } return(n1); }