static void SplitTriangleList(this kDOPCompact kdop, int idx, int start, int numTris, kDopBuildTriangle[] triangles, kDOP bound, kDOPCompact[] nodes) { int numRight = numTris / 2; int numLeft = numTris - numRight; int firstRight = start + numLeft; int bestPlane = FindBestPlane(triangles, start, numTris); PartialSort(start, start + numTris - 1, firstRight, triangles, PlaneNormals[bestPlane]); kDOP leftBound = new kDOP(); leftBound.AddTriangles(triangles, start, numLeft); kDOP rightBound = new kDOP(); rightBound.AddTriangles(triangles, firstRight, numRight); kdop.Compress(bound, leftBound, rightBound); int leftIdx = 2 * idx + 1; int rightIdx = leftIdx + 1; if (leftIdx < nodes.Length - 1) { nodes[leftIdx].SplitTriangleList(leftIdx, start, numLeft, triangles, leftBound, nodes); nodes[rightIdx].SplitTriangleList(rightIdx, firstRight, numRight, triangles, rightBound, nodes); } }
static void Compress(this kDOPCompact kdop, kDOP bound, kDOP left, kDOP right) { for (int i = 0; i < 3; ++i) { kdop.Min[i] = CompressAxis(bound.Min[i], bound.Max[i], left.Min[i], right.Min[i]); kdop.Max[i] = CompressAxis(bound.Max[i], bound.Min[i], left.Max[i], right.Max[i]); } }
static void AddPoint(this kDOP kdop, Vector3 point) { for (int i = 0; i < 3; i++) { var dotProd = Vector3.Dot(point, PlaneNormals[i]); if (dotProd < kdop.Min[i]) { kdop.Min[i] = dotProd; } if (dotProd > kdop.Max[i]) { kdop.Max[i] = dotProd; } } }
static void AddTriangles(this kDOP kdop, kDopBuildTriangle[] triangles, int start, int numTris) { for (int i = 0; i < 3; ++i) { kdop.Min[i] = float.MaxValue; kdop.Max[i] = float.MinValue; } for (int i = start; i < start + numTris; i++) { kdop.AddPoint(triangles[i].V0); kdop.AddPoint(triangles[i].V1); kdop.AddPoint(triangles[i].V2); } }
public static kDOPTreeCompact ToCompact(kDOPCollisionTriangle[] oldTriangles, Vector3[] vertices) { var rootBound = new kDOP { Min = new float[3], Max = new float[3] }; for (int i = 0; i < 3; i++) { rootBound.Max[i] = float.MaxValue; rootBound.Min[i] = -float.MaxValue; } if (oldTriangles.IsEmpty()) { return(new kDOPTreeCompact { RootBound = rootBound, Nodes = new kDOPCompact[0], Triangles = new kDOPCollisionTriangle[0] }); } var buildTriangles = new kDopBuildTriangle[oldTriangles.Length]; for (int i = 0; i < oldTriangles.Length; i++) { kDOPCollisionTriangle oldTri = oldTriangles[i]; buildTriangles[i] = new kDopBuildTriangle(oldTri, vertices[oldTri.Vertex1], vertices[oldTri.Vertex2], vertices[oldTri.Vertex3]); } int numNodes = 0; if (buildTriangles.Length > 5) { numNodes = 1; while ((buildTriangles.Length + numNodes - 1) / numNodes > 10) { numNodes *= 2; } numNodes = 2 * numNodes; } var nodes = new kDOPCompact[numNodes]; for (int i = 0; i < numNodes; i++) { nodes[i] = new kDOPCompact(); for (int j = 0; j < 3; j++) { nodes[i].Min[j] = 0; nodes[i].Max[j] = 0; } } rootBound.AddTriangles(buildTriangles, 0, buildTriangles.Length); if (numNodes > 1 && buildTriangles.Length > 1) { nodes[0].SplitTriangleList(0, 0, buildTriangles.Length, buildTriangles, rootBound, nodes); } return(new kDOPTreeCompact { RootBound = rootBound, Nodes = nodes, Triangles = buildTriangles.Select(tri => (kDOPCollisionTriangle)tri).ToArray() }); }