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