public bool PlanePlanePlaneIntersect(Plane p1, Plane p2, Plane p3, out Vector3 r) { r = Vector3.Zero; Vector3 m1 = new Vector3(p1.Normal.X, p2.Normal.X, p3.Normal.X); Vector3 m2 = new Vector3(p1.Normal.Y, p2.Normal.Y, p3.Normal.Y); Vector3 m3 = new Vector3(p1.Normal.Z, p2.Normal.Z, p3.Normal.Z); Vector3 u = Vector3.Cross(m2, m3); Single denom = Vector3.Dot(m1, u); if (Math.Abs(denom) < SMALL_NUM) { return false; } Vector3 d = new Vector3(p1.Distance, p2.Distance, p3.Distance); Vector3 v = Vector3.Cross(m1, d); Single ood = 1.0f / denom; r.X = Vector3.Dot(d, u) * ood; r.Y = Vector3.Dot(m3, v) * ood; r.Z = -Vector3.Dot(m2, v) * ood; return true; }
public void GenerateKDOP(int K = 6) { Vector3[] AABB = new Vector3[3]; AABB[0] = new Vector3(1, 0, 0); AABB[1] = new Vector3(0, 1, 0); AABB[2] = new Vector3(0, 0, 1); Vector3[] Corners = new Vector3[4]; Corners[0] = new Vector3(1, 1, 1); Corners[1] = new Vector3(1, -1, 1); Corners[2] = new Vector3(1, 1, -1); Corners[3] = new Vector3(1, -1, -1); Vector3[] Edges = new Vector3[6]; Edges[0] = new Vector3(1, 1, 0); Edges[1] = new Vector3(1, 0, 1); Edges[2] = new Vector3(0, 1, 1); Edges[3] = new Vector3(1, -1, 0); Edges[4] = new Vector3(1, 0, -1); Edges[5] = new Vector3(0, 1, -1); Vector3[] bounds = new Vector3[K / 2]; int offset = 0; if (K == 6 || K == 14 || K == 18 || K == 26) { Array.Copy(AABB, 0, bounds, offset, AABB.Length); offset += AABB.Length; } if (K == 8 || K == 14 || K == 26) { Array.Copy(Corners, 0, bounds, offset, Corners.Length); offset += Corners.Length; } if (K == 12 || K == 18 || K == 26) { Array.Copy(Edges, 0, bounds, offset, Edges.Length); offset += Edges.Length; } // Initialise output extents Plane[] min = new Plane[K / 2]; Plane[] max = new Plane[K / 2]; for (int i = 0; i < min.Length; i++) { min[i] = new Plane(Vector3.Zero, bounds[i], Single.MaxValue); max[i] = new Plane(Vector3.Zero, bounds[i], Single.MinValue); } Single value = 0; for (int i = 0; i < Verts.Count; i++) { for (int j = 0; j < bounds.Length; j++) { value = (Verts[i] * bounds[j]).Sum(); if (value < min[j].Distance) { min[j].Distance = value; min[j].Point = Verts[i]; } else if (value > max[j].Distance) { max[j].Distance = value; max[j].Point = Verts[i]; } } } Plane[] planes = new Plane[K]; for (int i = 0; i < min.Length; i++) { planes[2 * i] = new Plane(min[i].Point, -min[i].Normal, Vector3.Dot(-min[i].Normal, min[i].Point)); planes[2 * i + 1] = max[i]; } for (int i = 0; i < planes.Length; i++) { //Console.WriteLine(Vector3.Dot(planes[i].Normal, planes[i].Point) + " :: " + planes[i].Distance); Console.WriteLine(planes[i]); } Vector3 v; //for (int a = 0; a < planes.Length; a++) //{ // for (int b = a; b < planes.Length; b++) // { // for (int c = b; c < planes.Length; c++) // { // if (PlanePlanePlaneIntersect(planes[a], planes[b], planes[c], out v)) // { // Console.WriteLine(a + " :: " + b + " :: " + c); // intersectionPoints.Add(v); // } // } // } //} PlanePlanePlaneIntersect(planes[3], planes[7], planes[11], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[3], planes[11], planes[12], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[3], planes[12], planes[17], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[3], planes[17], planes[7], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[0], planes[11], planes[5], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[0], planes[16], planes[5], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[0], planes[12], planes[4], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[0], planes[6], planes[4], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[1], planes[7], planes[5], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[1], planes[13], planes[5], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[1], planes[17], planes[4], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[1], planes[10], planes[4], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[2], planes[13], planes[16], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[2], planes[16], planes[6], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[2], planes[6], planes[10], out v); intersectionPoints.Add(v); PlanePlanePlaneIntersect(planes[2], planes[10], planes[13], out v); intersectionPoints.Add(v); }