空间中任意多边形相交算法

        public static string Mypf_Function()
        {
            var lee = 0;
            float d1 = 0, d2 = 0;
            var ret = "多面体不相交！！";

            //空间两多面体
            p1[0] = new PolytopePoint(0, 0, 1);
            p1[1] = new PolytopePoint(0, 0, 2);
            p1[2] = new PolytopePoint(2, 2, 2);
            p1[3] = new PolytopePoint(2, 2, 1);

            p2[0] = new PolytopePoint(2, 1, 0);
            p2[1] = new PolytopePoint(0, 1, 0);
            p2[2] = new PolytopePoint(0, 1, 3);
            p2[3] = new PolytopePoint(2, 1, 3);

            var x1 = p1[1].X - p1[0].X;
            var y1 = p1[1].Y - p1[0].Y;
            var z1 = p1[1].Z - p1[0].Z;

            var x2 = p1[2].X - p1[0].X;
            var y2 = p1[2].Y - p1[0].Y;
            var z2 = p1[2].Z - p1[0].Z;

            var a1 = p2[1].X - p2[0].X;
            var b1 = p2[1].Y - p2[0].Y;
            var c1 = p2[1].Z - p2[0].Z;

            var a2 = p2[2].X - p2[0].X;
            var b2 = p2[2].Y - p2[0].Y;
            var c2 = p2[2].Z - p2[0].Z;

            //两面法向量
            var n1 = new PolytopePoint(y1 * z2 - z1 * y2, z1 * x2 - x1 * z2, x1 * y2 - y1 * x2);
            var n2 = new PolytopePoint(b1 * c2 - c1 * b2, c1 * a2 - a1 * c2, a1 * b2 - b1 * a2);

            //第一平面点到第二平面的有符号距离
            var a=new float[4];
            for (var i = 0; i < 4; i++)
            {
                a[i] = n1.X * (p2[i].X - p1[0].X) + n1.Y * (p2[i].Y - p1[0].Y) + n1.Z * (p2[i].Z - p1[0].Z);
            }

            //不相交条件
            if (a[0] * a[1] > 0 && a[0] * a[2] > 0 && a[0] * a[3] > 0)
            {
                return ret;
            }

            //共面
            if (Math.Abs(a[0]) < 0.00000000000001 && Math.Abs(a[1]) < 0.00000000000001 && Math.Abs(a[2]) < 0.00000000000001 && Math.Abs(a[3]) < 0.00000000000001)
            {
                ret = "两多面体共面！";
                return ret;
            }

            //第一平面点到第一平面的有符号距离
            var b = new float[4];
            for (var i = 0; i < 4; i++)
            {
                b[i] = n2.X * (p1[i].X - p2[0].X) + n2.Y * (p1[i].Y - p2[0].Y) + n2.Z * (p1[i].Z - p2[0].Z);
            }

            for (var i = 0; i < 3; i++)
            {
                for (var j = 0; j < 3; j++)
                {
                    //第二平面上的夹边边对存在条件
                    if (a[i] * a[i + 1] < 0)
                    {
                        //第一平面上的夹边边对存在条件
                        if (b[j] * b[j + 1] < 0)
                        {
                            var l = p1[j + 1].X - p1[j].X;
                            var m = p1[j + 1].Y - p1[j].Y;
                            var n = p1[j + 1].Z - p1[j].Z; 

                            var o = p2[i + 1].X - p2[i].X;
                            var p = p2[i + 1].Y - p2[i].Y;
                            var q = p2[i + 1].Z - p2[i].Z;

                            //一二面上各取一条夹边组成边对与向量nt垂直
                            var nt=new PolytopePoint(m*q-n*p,n*o-l*q,l*p-m*o);

                            if (lee == 0)
                            {
                                d1 = nt.X*(p1[i].X - p2[j].X) + nt.Y*(p1[i].Y - p2[j].Y) + nt.Z*(p1[i].Z - p2[j].Z);
                                lee++;
                            }
                            else
                            {
                                d2 = nt.X * (p1[i].X - p2[j].X) + nt.Y * (p1[i].Y - p2[j].Y) + nt.Z * (p1[i].Z - p2[j].Z);
                            }

                            //面相交的条件
                            if (d1 * d2 < 0 || (Math.Abs(d1) < 0.00000000000001 && Math.Abs(d2) < 0.00000000000001))    //稍有缺陷  把条件放宽泛至只要有线相交多面体就相交
                            {
                                ret = "两多面体相交！";
                                return ret;
                            }
                        }
                    }
                }
            }

            ret = "异常";
            return ret;
        }