ulong GetID(ref TriangleVertexIndicesKD triangle)
{
ulong id0 = (ushort)triangle.I0;
ulong id1 = (ushort)triangle.I1;
ulong id2 = (ushort)triangle.I2;
ulong ID = (ulong)id0;
int shift = sizeof(ushort) * 8;
ID |= (id1 << shift);
ID |= (id2 << (shift * 2));
return ID;
}
void ConstructKDTree(KDNode currNode, int depth, TriangleVertexIndicesKD[] entities)
{
if (entities.Length == 0)
return;
int axis = depth % maxDimension;
List<TriangleVertexIndicesKD> sortedList = new List<TriangleVertexIndicesKD>();
sortedList.AddRange(entities);
switch (axis)
{
case 0:
sortedList.Sort(CompareX);
break;
case 1:
sortedList.Sort(CompareY);
break;
case 2:
sortedList.Sort(CompareZ);
break;
}
BoundingBox bounds = new BoundingBox(vertices[sortedList[0].I0], vertices[sortedList[0].I0]);
Vector3[] vecs = new Vector3[3];
for (int i = 0; i < sortedList.Count; i++)
{
vecs[0] = vertices[sortedList[i].I0];
vecs[1] = vertices[sortedList[i].I1];
vecs[2] = vertices[sortedList[i].I2];
for (int j = 0; j < vecs.Length; j++)
{
bounds.Max = Vector3.Max(bounds.Max, vecs[j]);
bounds.Min = Vector3.Min(bounds.Min, vecs[j]);
}
}
int medianIndex = sortedList.Count / 2;
vecs[0] = vertices[sortedList[medianIndex].I0];
vecs[1] = vertices[sortedList[medianIndex].I1];
vecs[2] = vertices[sortedList[medianIndex].I2];
BoundingBox triBounds = new BoundingBox(vecs[0], vecs[0]);
for (int j = 0; j < vecs.Length; j++)
{
triBounds.Max = Vector3.Max(triBounds.Max, vecs[j]);
triBounds.Min = Vector3.Min(triBounds.Min, vecs[j]);
}
float extra = 1.000001f;
bounds.Min = bounds.Min * extra;
bounds.Max = bounds.Max * extra;
triBounds.Min = triBounds.Min * extra;
triBounds.Max = triBounds.Max * extra;
if (sortedList.Count > 1)
{
depth++;
int leftCount = medianIndex;
if (leftCount > 0)
{
TriangleVertexIndicesKD[] leftEntities = new TriangleVertexIndicesKD[leftCount];
sortedList.CopyTo(0, leftEntities, 0, leftCount);
currNode.leftChild = new KDNode();
ConstructKDTree(currNode.leftChild, depth, leftEntities);
}
int rightCount = sortedList.Count - (medianIndex + 1);
if (rightCount > 0)
{
TriangleVertexIndicesKD[] rightEntities = new TriangleVertexIndicesKD[rightCount];
sortedList.CopyTo(medianIndex + 1, rightEntities, 0, rightCount);
currNode.rightChild = new KDNode();
ConstructKDTree(currNode.rightChild, depth, rightEntities);
}
}
currNode.element = sortedList[medianIndex];
currNode.boundingBox = bounds;
currNode.triangleBox = triBounds;
}
void ConvertToKD(List<TriangleVertexIndices> indices, List<Vector3> vertices)
{
this.vertices = vertices;
this.indices = new List<TriangleVertexIndicesKD>(indices.Count);
this.triMap = new SortedList<ulong, int>();
for (int i = 0; i < indices.Count; i++)
{
TriangleVertexIndices currTri = indices[i];
Vector3[] vecs = new Vector3[3];
vecs[0] = vertices[currTri.I0];
vecs[1] = vertices[currTri.I1];
vecs[2] = vertices[currTri.I2];
Vector3 sum = vecs[0];
Vector3.Add(ref sum, ref vecs[1], out sum);
Vector3.Add(ref sum, ref vecs[2], out sum);
Vector3.Multiply(ref sum, 1.0f / 3.0f, out sum);
Vector3 center = sum;
Vector3 normal;
Vector3.Subtract(ref vecs[1], ref vecs[0], out sum);
Vector3.Subtract(ref vecs[2], ref vecs[0], out normal);
Vector3.Cross(ref sum, ref normal, out normal);
TriangleVertexIndicesKD newTri = new TriangleVertexIndicesKD(currTri.I0, currTri.I1, currTri.I2, vecs[0], normal);
this.triMap.Add(GetID(ref newTri), i);
this.indices.Add(newTri);
}
}
private static int CompareZ(TriangleVertexIndicesKD elemA, TriangleVertexIndicesKD elemB)
{
if (elemA.Centroid.Z < elemB.Centroid.Z)
return -1;
if (elemA.Centroid.Z > elemB.Centroid.Z)
return 1;
return 0;
}
