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