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);
}
