using System;
using System.Collections.Generic;
using System.Text;
using System.Windows.Forms;
using System.Drawing;


namespace CSGBuilder {

    class Operations {

        public static double EPS = 0.000001;
        public static Random rand = new Random();

        //
        // Megadja, hogy az x, y, z koodinátákkal adott pont benne van-e 
        // a p2, q2 pontokkal adott téglatestben
        //
        private static bool InsideBox(double x, double y, double z, Point3D p, Point3D q) {
            return (x >= p.X - EPS && x <= q.X + EPS && y >= p.Y - EPS && y <= q.Y + EPS && z >= p.Z - EPS && z <= q.Z + EPS) ||
                (x <= p.X + EPS && x >= q.X - EPS && y <= p.Y + EPS && y >= q.Y - EPS && z <= p.Z + EPS && z >= q.Z - EPS);
        }

        //
        // Megadja, hogy a p1, q1 pontokkal adott téglatest metszi-e a p2, q2 pontokkal adottat
        //
        private static bool BoxIntersect(Point3D p1, Point3D q1, Point3D p2, Point3D q2) {
            /*
            return InsideBox(p1.X, p1.Y, p1.Z, p2, q2) ||
                   InsideBox(q1.X, p1.Y, p1.Z, p2, q2) ||
                   InsideBox(q1.X, q1.Y, p1.Z, p2, q2) ||
                   InsideBox(p1.X, q1.Y, p1.Z, p2, q2) ||
                   InsideBox(p1.X, p1.Y, q1.Z, p2, q2) ||
                   InsideBox(q1.X, p1.Y, q1.Z, p2, q2) ||
                   InsideBox(q1.X, q1.Y, q1.Z, p2, q2) ||
                   InsideBox(p1.X, q1.Y, q1.Z, p2, q2); */
            return (q1.X > p2.X || q2.X > p1.X) && (q1.Y > p2.Y || q2.Y > p1.Y) && (q1.Z > p2.Z || q2.Z > p1.Z);
        }

        //
        // Megadja, hogy egy pont benne van-e egy tetraéderben
        // visszatérési érték: 
        //   1, benne van
        //  -1, nincs benne
        //   0, a pont a test felszínén van
        //
        private static int InsideTetrahedron(Point3D q, Point3D a, Point3D b, Point3D c) {
            double det_abq = (a.X * b.Y * q.Z + a.Y * b.Z * q.X + a.Z * b.X * q.Y -
                             a.Z * b.Y * q.X - a.X * b.Z * q.Y - a.Y * b.X * q.Z);
            double det_acq = (a.X * c.Y * q.Z + a.Y * c.Z * q.X + a.Z * c.X * q.Y -
                             a.Z * c.Y * q.X - a.X * c.Z * q.Y - a.Y * c.X * q.Z);
            double det_bcq = (b.X * c.Y * q.Z + b.Y * c.Z * q.X + b.Z * c.X * q.Y -
                             b.Z * c.Y * q.X - b.X * c.Z * q.Y - b.Y * c.X * q.Z);
            double det_abc = (a.X * b.Y * c.Z + a.Y * b.Z * c.X + a.Z * b.X * c.Y -
                             a.Z * b.Y * c.X - a.X * b.Z * c.Y - a.Y * b.X * c.Z);
            double det_abcq = det_abc - det_abq + det_acq - det_bcq;
            //if (det_abcq > -EPS && det_abcq < EPS) return 0;
            //if (det_abcq > 0) return 1; else return -1;
            if (det_abq > 0 && -det_acq > 0 && det_bcq > 0 && det_abcq > 0) return 1; else return -1;
            //if (det_abc < 0.0) return -1;
            //return 0;
        }

        //
        // Megadja, hogy egy q pont benne van-e egy p testben
        // visszatérési érték:
        //  -1 : a pont nincs benne a testben
        //   0 : a pont a test határán van
        //   1 : a pont benne van a testben
        // 
        private static int InsideSolid2(Point3D q, Solid p, int facenum) {
            // Ha a pont nincs benne a test befoglaló dobozában, akkor nem lehet a testben sem
            if (!InsideBox(q.X, q.Y, q.Z, p.LowerCorner, p.UpperCorner)) return -1;
            int sum = 0, n;
            Face f;
            // Végigmegyünk az összes lapon
            for (int i = 0; i < p.Mesh.Faces.Count; i++) {
                // Ha a pont benne van a lap és az origo által meghatározott tetraéderben,
                // akkor hozzáadjuk a +1, -1 vagy 0 értéket a végösszeghez
                f = p.Mesh.Faces[i];
                //if (PointInTriangle3D(q, p.Mesh.Vertices[f[0]], p.Mesh.Vertices[f[1]], p.Mesh.Vertices[f[2]])) return 0;
                n = InsideTetrahedron(q, p.Mesh.Vertices[f[0]], p.Mesh.Vertices[f[1]], p.Mesh.Vertices[f[2]]);
                if (n == 0) return 0;
                sum += n;
            }
            // Ha az összeg > 0, akkor a pont benne van a testben, különben nem
            if (sum > 0) return 1;
            else return -1;
            //if (sum == 0) return 0; else return -1;
        }

        private static int InsideSolid(Point3D q, Solid p, int facenum) {
            // Ha a pont nincs benne a test befoglaló dobozában, akkor nem lehet a testben sem
            if (!InsideBox(q.X, q.Y, q.Z, p.LowerCorner, p.UpperCorner)) return -1;
            int sum = 0;
            Point3D t, q1 = new Point3D(q.X + rand.NextDouble() * 10, q.Y + rand.NextDouble() * 10, q.Z + rand.NextDouble() * 10);
            Point3D a, b, c, n;
            Face f;
            // Végigmegyünk az összes lapon
            for (int i = 0; i < p.Mesh.Faces.Count; i++) {
                f = p.Mesh.Faces[i];
                a = p.Mesh.Vertices[f[0]]; b = p.Mesh.Vertices[f[1]]; c = p.Mesh.Vertices[f[2]];
                n = (b - a) % (c - a); n.Normal();
                if (PointInTriangle3D(q, a, b, c, n)) return 0;
                double denom = (n * (q1 - q));
                if (Math.Abs(denom) < EPS) continue;
                double ri = (n * (p.Mesh.Vertices[f[0]] - q)) / denom;
                if (ri <= 0.0) continue;
                t = q + (q1 - q) * ri;
                if (PointInTriangle(t, a, b, c))
                    sum += 1;
            }
            if ((sum % 2) == 1) return 1; else return -1;
        }

        private static bool PointInTriangle3D(Point3D p, Point3D a, Point3D b, Point3D c, Point3D n) {
            if (Math.Abs(n * (p - a)) < EPS)
                if (PointInTriangle(p, a, b, c)) return true;
            return false;
        }

        //
        // Megadja, hogy a p1, p2, p3 pontok közül melyik esik a q ponttal és n normálvektorral adott
        // sík többi ponttal ellentétes oldalára. Ha mindhárom pont azonos oldalra esik, -1-et ad vissza.
        //
        private static int TrianglePosition(Point3D p1, Point3D p2, Point3D p3, Point3D q, Point3D n) {
            int sgn1;
            double s1 = n * (p1 - q);
            if (s1 < -EPS) sgn1 = -1;
            else
                if (s1 > EPS) sgn1 = 1; else sgn1 = 0;
            int sgn2;
            double s2 = n * (p2 - q);
            if (s2 < -EPS) sgn2 = -1;
            else
                if (s2 > EPS) sgn2 = 1; else sgn2 = 0;
            int sgn3;
            double s3 = n * (p3 - q);
            if (s3 < -EPS) sgn3 = -1;
            else
                if (s3 > EPS) sgn3 = 1; else sgn3 = 0;

            if (sgn1 == 0 && sgn2 == 0 && sgn3 == 0) return 0; // mindhárom pont az adott síkon van
            if (sgn1 == sgn2 && sgn2 == sgn3) return -1;  // mindhárom pont a sík azonos oldalán van
            //if (sgn1 == 0 && (sgn2 * sgn3 == 0 || sgn2 == sgn3)) return -1;
            //if (sgn2 == 0 && (sgn1 * sgn3 == 0 || sgn1 == sgn3)) return -1;
            //if (sgn3 == 0 && (sgn1 * sgn2 == 0 || sgn1 == sgn2)) return -1;
            if (sgn1 == 0 && sgn2 == 0) return -1;
            if (sgn1 == 0 && sgn3 == 0) return -1;
            if (sgn3 == 0 && sgn2 == 0) return -1;
            if (sgn1 == sgn2) return (sgn3 == 0 || sgn3 == sgn1) ? -1 : 3;
            if (sgn1 == sgn3) return (sgn2 == 0 || sgn2 == sgn1) ? -1 : 2;
            if (sgn3 == sgn2) return (sgn1 == 0 || sgn1 == sgn3) ? -1 : 1;
            if (sgn1 != 0) return 1;
            if (sgn2 != 0) return 2;
            else return 3;

        }

        private static bool SameSide(Point3D p1, Point3D p2, Point3D a, Point3D b) {
            Point3D cp1 = (b - a) % (p1 - a);
            Point3D cp2 = (b - a) % (p2 - a);
            if ((cp1 * cp2) >= EPS) return true;
            else return false;
        }

        private static bool PointInTriangle(Point3D p, Point3D a, Point3D b, Point3D c) {
            Point3D u = b - a;
            Point3D v = c - a;
            Point3D w = p - a;
            double si, ti;
            double uv = (u * v), wv = (w * v), vv = (v * v), wu = (w * u), uu = (u * u);
            double denom = (uv * uv - uu * vv);
            if (Math.Abs(denom) < EPS) return false;
            si = (uv * wv - vv * wu) / denom;
            ti = (uv * wu - uu * wv) / denom;            
            if (si >= -EPS && ti >= -EPS && si + ti <= 1.0 + EPS) return true; else return false;
        }

        /*
        Calculate the line segment PaPb that is the shortest route between
        two lines P1P2 and P3P4. Calculate also the values of mua and mub where
            Pa = P1 + mua (P2 - P1)
            Pb = P3 + mub (P4 - P3)
        Return FALSE if no solution exists.
        */
        private static bool LineLineIntersect(Point3D p1, Point3D p2, Point3D p3, Point3D p4, out Point3D pa, out double mua) {
            Point3D p13, p43, p21, pb;
            double d1343, d4321, d1321, d4343, d2121;
            double numer, denom;
            double mub;

            pa = null; mua = 0.0;
            p13 = p1 - p3;
            p43 = p4 - p3;
            if (Math.Abs(p43.X) < EPS && Math.Abs(p43.Y) < EPS && Math.Abs(p43.Z) < EPS)
                return false;
            p21 = p2 - p1;
            if (Math.Abs(p21.X) < EPS && Math.Abs(p21.Y) < EPS && Math.Abs(p21.Z) < EPS)
                return false;

            d1343 = p13 * p43;
            d4321 = p43 * p21;
            d1321 = p13 * p21;
            d4343 = p43 * p43;
            d2121 = p21 * p21;

            denom = d2121 * d4343 - d4321 * d4321;
            if (Math.Abs(denom) < EPS)
                return false;
            numer = d1343 * d4321 - d1321 * d4343;

            mua = numer / denom;
            mub = (d1343 + d4321 * mua) / d4343;

            pa = new Point3D(p1.X + mua * p21.X, p1.Y + mua * p21.Y, p1.Z + mua * p21.Z);
            pb = new Point3D(p3.X + mub * p43.X, p3.Y + mub * p43.Y, p3.Z + mub * p43.Z);

            //if (Equal(pa, pb)) return true; else return false;
            return false;
        }

        //
        // Megadja két háromszög metszésvonalának két végpontját (i1, i2), 
        // és hogy melyik csúcs esik a metszési síknak a másik kettővel ellentétes oldalára
        // Ha nem metszik egymást, a visszatérési érték FALSE.
        //
        private static bool TriangleIntersection(
                Point3D p1, Point3D p2, Point3D p3,  // Az első háromszög csúcsai
                Point3D q1, Point3D q2, Point3D q3,  // A második háromszög csúcsai
                out Point3D i1, out Point3D i2, out int oddp) {
            oddp = -1; i1 = i2 = null;
            //if (TrianglePosition(q1, q2, q3, p1, (p2 - p1) % (p3 - p1)) == -1) return false;  // !gyorsítás
            Point3D pa, pb, pc, n = (q2 - q1) % (q3 - q1); // a 2. hsz síkjának normálvektora            
            double s, npa, npb, npc;
            oddp = TrianglePosition(p1, p2, p3, q1, n);
            if (oddp == -1) return false;  // Ha p1, p2, p3 azonos oldalra esik, nincs metszéspont            
            switch (oddp) {    // különben 3 lehetséges eset van
                case 0:
                    if (!PointInTriangle(p1, q1, q2, q3) && !PointInTriangle(p2, q1, q2, q3) && !PointInTriangle(p3, q1, q2, q3)) return false;
                    // A két háromszög azonos síkban van  
                    double mua;
                    // Teszt a p1p2 szakaszra
                    if (LineLineIntersect(p1, p2, q1, q2, out i1, out mua) && mua >= 0 && mua <= 1) {
                        if (LineLineIntersect(p1, p3, q1, q2, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 10; return true; }
                        if (LineLineIntersect(p2, p3, q1, q2, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 11; return true; }
                    }
                    if (LineLineIntersect(p1, p2, q1, q3, out i1, out mua) && mua >= 0 && mua <= 1) {
                        if (LineLineIntersect(p1, p3, q1, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 10; return true; }
                        if (LineLineIntersect(p2, p3, q1, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 11; return true; }
                    }
                    if (LineLineIntersect(p1, p2, q2, q3, out i1, out mua) && mua >= 0 && mua <= 1) {
                        if (LineLineIntersect(p1, p3, q2, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 10; return true; }
                        if (LineLineIntersect(p2, p3, q2, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 11; return true; }
                    }
                    // Teszt a p1p3 szakaszra
                    if (LineLineIntersect(p1, p3, q1, q2, out i1, out mua) && mua >= 0 && mua <= 1) {
                        if (LineLineIntersect(p1, p2, q1, q2, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 10; return true; }
                        if (LineLineIntersect(p2, p3, q1, q2, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 12; return true; }
                    }
                    if (LineLineIntersect(p1, p3, q1, q3, out i1, out mua) && mua >= 0 && mua <= 1) {
                        if (LineLineIntersect(p1, p2, q1, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 10; return true; }
                        if (LineLineIntersect(p2, p3, q1, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 12; return true; }
                    }
                    if (LineLineIntersect(p1, p3, q2, q3, out i1, out mua) && mua >= 0 && mua <= 1) {
                        if (LineLineIntersect(p1, p2, q2, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 10; return true; }
                        if (LineLineIntersect(p2, p3, q2, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 12; return true; }
                    }
                    // Teszt a p2p3 szakaszra
                    if (LineLineIntersect(p2, p3, q1, q2, out i1, out mua) && mua >= 0 && mua <= 1) {
                        if (LineLineIntersect(p1, p3, q1, q2, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 12; return true; }
                        if (LineLineIntersect(p1, p2, q1, q2, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 11; return true; }
                    }
                    if (LineLineIntersect(p2, p3, q1, q3, out i1, out mua) && mua >= 0 && mua <= 1) {
                        if (LineLineIntersect(p1, p3, q1, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 12; return true; }
                        if (LineLineIntersect(p1, p2, q1, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 11; return true; }
                    }
                    if (LineLineIntersect(p2, p3, q2, q3, out i1, out mua) && mua >= 0 && mua <= 1) {
                        if (LineLineIntersect(p1, p3, q2, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 12; return true; }
                        if (LineLineIntersect(p1, p2, q2, q3, out i2, out mua) && mua >= 0 && mua <= 1) { oddp = 11; return true; }
                    }
                    return false;
                case 1:
                    // A p1 pont van "külön", ezért a belőle induló élek metszik a síkot
                    pa = p2 - p1;
                    pb = q1 - p1;
                    pc = p3 - p1;
                    npa = n * pa;
                    npb = n * pb;
                    if (Math.Abs(npa) < EPS) { //return false;
                        i1 = p2; 
                    } else {
                        s = npb / npa; i1 = p1 + pa * s;
                    }
                    npc = n * pc;
                    if (Math.Abs(npc) < EPS) {//return false;
                        i2 = p3; 
                    } else {
                        s = npb / npc; i2 = p1 + pc * s;
                    }
                    break;
                case 2:
                    // A p2 pont van "külön", ezért a belőle induló élek metszik a síkot
                    pa = p1 - p2;
                    pb = q1 - p2;
                    pc = p3 - p2;
                    npa = n * pa;
                    npb = n * pb;
                    if (Math.Abs(npa) < EPS) {//return false;
                        i1 = p1; 
                    } else {
                        s = npb / npa; i1 = p2 + pa * s;
                    }
                    npc = n * pc;
                    if (Math.Abs(npc) < EPS) {//return false;
                        i2 = p3; 
                    } else {
                        s = npb / npc; i2 = p2 + pc * s;
                    }
                    break;
                case 3:
                    // A p3 pont van "külön", ezért a belőle induló élek metszik a síkot
                    pa = p1 - p3;
                    pb = q1 - p3;
                    pc = p2 - p3;
                    npa = n * pa;
                    npb = n * pb;
                    if (Math.Abs(npa) < EPS) {//return false;
                        i1 = p1;
                    } else {
                        s = npb / npa; i1 = p3 + pa * s;
                    }
                    npc = n * pc;
                    if (Math.Abs(npc) < EPS) {//return false;
                        i2 = p2;
                    } else {
                        s = npb / npc; i2 = p3 + pc * s;
                    }
                    break;
            }
            // Ha a két metszéspont közül legalább az egyik belülre esik, akkor 
            // a két háromszög tényleg metszi egymást
            double m;
            Point3D p;
            bool inside = PointInTriangle(i1, q1, q2, q3) || PointInTriangle(i2, q1, q2, q3);
            //if (!inside) inside = !SameSide(i1, i2, q1, q2) || !SameSide(i1, i2, q1, q3) || !SameSide(i1, i2, q2, q3);
            if (inside) return true; else return false;            
        }

        public static bool Equal(Point3D p1, Point3D p2) {
            return (Math.Abs(p1.X - p2.X) < EPS && Math.Abs(p1.Y - p2.Y) < EPS && Math.Abs(p1.Z - p2.Z) < EPS);
        }

        //
        // Két test közötti halmazművelet - únió, metszet, különbség
        //
        public static Solid Construct(string type, Solid p1, Solid p2, ProgressForm pf) {
            Solid result = new Solid();
            result.Mesh = new BRep();
            result.Center = //(p1.Center + p2.Center) / 2;
                new Point3D(0, 0, 0);
            result.LowerCorner = new Point3D(
                p1.LowerCorner.X < p2.LowerCorner.X ? p1.LowerCorner.X : p2.LowerCorner.X,
                p1.LowerCorner.Y < p2.LowerCorner.Y ? p1.LowerCorner.Y : p2.LowerCorner.Y,
                p1.LowerCorner.Z < p2.LowerCorner.Z ? p1.LowerCorner.Z : p2.LowerCorner.Z);
            result.UpperCorner = new Point3D(
                p1.UpperCorner.X > p2.UpperCorner.X ? p1.UpperCorner.X : p2.UpperCorner.X,
                p1.UpperCorner.Y > p2.UpperCorner.Y ? p1.UpperCorner.Y : p2.UpperCorner.Y,
                p1.UpperCorner.Z > p2.UpperCorner.Z ? p1.UpperCorner.Z : p2.UpperCorner.Z);
            result.E1 = new Point3D(result.UpperCorner.X, 0.0, 0.0);
            result.E2 = new Point3D(0.0, result.UpperCorner.Y, 0.0);
            result.E3 = new Point3D(0.0, 0.0, result.UpperCorner.Z);
            if (!BoxIntersect(p1.LowerCorner, p1.UpperCorner, p2.LowerCorner, p2.UpperCorner)) {
                if (type == "union") {
                    // A befoglaló téglatestek nem metszik egymást, így a testek sem metszhetik egymást,
                    // az eredmény a pontok, élek és lapok halmazának úniója lesz, nem kell vágni
                    foreach (Point3D pt in p1.Mesh.Vertices)
                        result.Mesh.Vertices.Add(pt.Clone());
                    foreach (Edge e in p1.Mesh.Edges)
                        result.Mesh.Edges.Add(e.Clone());
                    foreach (Face f in p1.Mesh.Faces)
                        result.Mesh.Faces.Add(f.Clone());
                    int vn = p1.Mesh.Vertices.Count;
                    foreach (Point3D pt in p2.Mesh.Vertices)
                        result.Mesh.Vertices.Add(pt.Clone());
                    foreach (Edge e in p2.Mesh.Edges) {
                        Edge e2 = new Edge(e.Sp + vn, e.Ep + vn); e2.Color = e.Color;
                        result.Mesh.Edges.Add(e2);
                    }
                    foreach (Face f in p2.Mesh.Faces) {
                        Face f2 = new Face(f[0] + vn, f[1] + vn, f[2] + vn); f2.Color = f.Color;
                        result.Mesh.Faces.Add(f2);
                    }
                    if (result.Mesh.Vertices.Count == 0) return null; else return result;
                }
                if (type == "intersect") {
                    // A befoglaló téglatestek nem metszik egymást, így a testek sem metszhetik egymást,
                    // az eredmény üres halmaz
                    return null;
                } else { // type == "difference"
                    // A befoglaló téglatestek nem metszik egymást, így a testek sem metszhetik egymást,
                    // az eredmény az első primitív lesz
                    foreach (Point3D pt in p1.Mesh.Vertices)
                        result.Mesh.Vertices.Add(pt.Clone());
                    foreach (Edge e in p1.Mesh.Edges)
                        result.Mesh.Edges.Add(e.Clone());
                    foreach (Face f in p1.Mesh.Faces)
                        result.Mesh.Faces.Add(f.Clone());
                    if (result.Mesh.Vertices.Count == 0) return null; else return result;
                }
            } else {
                // A két test metszi egymást, vágni kell
                int p1v = p1.Mesh.Vertices.Count, p1f = p1.Mesh.Faces.Count, p1e = p1.Mesh.Edges.Count;
                int p2v = p2.Mesh.Vertices.Count, p2f = p2.Mesh.Faces.Count, p2e = p2.Mesh.Edges.Count;
                Point3D i1, i2, a, b, c;
                int oddp, ai, bi, ci;
                bool intersect;
                pf.lCompleted.Text = "Constructing the solid...";
                for (int i = 0; i < p1.Mesh.Faces.Count; i++) {
                    if (pf.IsDisposed) return null;
                    int val = (int)((double)i / (double)p1.Mesh.Faces.Count * 50.0);
                    if (pf.progBar.Value < val) pf.progBar.Value = val;
                    pf.progBar.Refresh();
                    ai = p1.Mesh.Faces[i][0];
                    a = p1.Mesh.Vertices[ai];
                    bi = p1.Mesh.Faces[i][1];
                    b = p1.Mesh.Vertices[bi];
                    ci = p1.Mesh.Faces[i][2];
                    c = p1.Mesh.Vertices[ci];
                    intersect = false;
                    Face.defaultColor = p1.Mesh.Faces[i].Color;
                    Point3D.defaultColor = p1.Mesh.Faces[i].Color;
                    for (int j = 0; j < p2.Mesh.Faces.Count; j++) {
                        // Ha a vizsgált hsz nem metszi a másik test egyetlen lapját sem, az azt jelenti,
                        // hogy teljes egészében kívül vagy belül van a testen.
                        if (TriangleIntersection(a, b, c,
                            p2.Mesh.Vertices[p2.Mesh.Faces[j][0]],
                            p2.Mesh.Vertices[p2.Mesh.Faces[j][1]],
                            p2.Mesh.Vertices[p2.Mesh.Faces[j][2]],
                            out i1, out i2, out oddp)) {
                            // Van metszés, ekkor a vizsgált hsz lapot el kell vágni, létrehozni az új lapokat
                            // és azokat is megvizsgálni, amíg az osztályozás egyértelmű nem lesz.
                            intersect = true;
                            p1.Mesh.Vertices.Add(i1);
                            int ind1 = p1.Mesh.Vertices.Count - 1;
                            p1.Mesh.Vertices.Add(i2);
                            int ind2 = p1.Mesh.Vertices.Count - 1;
                            Face f;
                            switch (oddp) {
                                case 1:
                                    if (Equal(i1, b) && Equal(i2, c)) break;
                                    else if (Equal(i1, b)) {
                                        f = new Face(ai, ind1, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(ind1, ci, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                    } else if (Equal(i2, c)) {
                                        f = new Face(ai, ind1, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(ind1, bi, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                    } else {
                                        f = new Face(ai, ind1, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(bi, ind2, ind1);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(ci, ind2, bi);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                    }
                                    break;
                                case 2:
                                    if (Equal(i1, a) && Equal(i2, c)) break;
                                    else if (Equal(i1, a)) {
                                        f = new Face(ind1, bi, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(ind1, ind2, ci);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                    } else if (Equal(i2, c)) {
                                        f = new Face(ind1, bi, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(ind1, ind2, ai);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                    } else {
                                        f = new Face(bi, ind2, ind1);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(ai, ind2, ci);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(ai, ind1, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                    }
                                    break;
                                default:
                                    if (Equal(i1, a) && Equal(i2, b)) break;
                                    else if (Equal(i1, a)) {
                                        f = new Face(ci, ind1, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(ind1, bi, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                    } else if (Equal(i2, b)) {
                                        f = new Face(ci, ind1, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(ind1, ai, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                    } else {
                                        f = new Face(ci, ind1, ind2);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(ai, ind2, ind1);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                        f = new Face(bi, ind2, ai);
                                        /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                        p1.Mesh.Faces.Add(f);
                                    }
                                    break;
                                case 10:
                                    if (((b - a) % (c - a)) * ((i1 - a) % (i2 - a)) < -EPS) {
                                        Point3D temp = i1; i1 = i2; i2 = temp;
                                    }
                                    f = new Face(ai, ind1, ind2);
                                    /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                    p1.Mesh.Faces.Add(f);
                                    f = new Face(bi, ci, ind1);
                                    /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                    p1.Mesh.Faces.Add(f);
                                    f = new Face(ci, ind2, ind1);
                                    /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                    p1.Mesh.Faces.Add(f);
                                    break;
                                case 11:
                                    if (((a - b) % (c - b)) * ((i1 - b) % (i2 - b)) < -EPS) {
                                        Point3D temp = i1; i1 = i2; i2 = temp;
                                    }
                                    f = new Face(bi, ind2, ind1);
                                    /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                    p1.Mesh.Faces.Add(f);
                                    f = new Face(ind2, ci, ind1);
                                    /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                    p1.Mesh.Faces.Add(f);
                                    f = new Face(ci, ai, ind1);
                                    /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                    p1.Mesh.Faces.Add(f);
                                    break;
                                case 12:
                                    if (((a - c) % (b - c)) * ((i2 - c) % (i1 - c)) < -EPS) {
                                        Point3D temp = i1; i1 = i2; i2 = temp;
                                    }
                                    f = new Face(ai, ind1, ind2);
                                    /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                    p1.Mesh.Faces.Add(f);
                                    f = new Face(bi, ind1, ai);
                                    /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                    p1.Mesh.Faces.Add(f);
                                    f = new Face(ci, ind2, ind1);
                                    /*if (!FaceExists(p1.Mesh.Faces, f))*/
                                    p1.Mesh.Faces.Add(f);
                                    break; 
                            }
                        }
                        if (intersect) break;
                    }
                    if (intersect) continue;
                    // Ha mindhárom pontja kívül van a másik testen, akkor a teljes háromszög 
                    // kívül van a másik testen, így bekerül az eredménybe
                    int b1 = InsideSolid(a, p2, p2f);
                    int b2 = InsideSolid(b, p2, p2f);
                    int b3 = InsideSolid(c, p2, p2f);
                    Point3D s = (a + b + c) / 3;
                    int b4 = InsideSolid(s, p2, p2f);
                    bool cond = false;
                    switch (type) {
                        case "union": cond = (b1 <= 0 && b2 <= 0 && b3 <= 0 && b4 <= 0); break;
                        case "intersection": cond = (b1 >= 0 && b2 >= 0 && b3 >= 0 && b4 >= 0); break;
                        case "difference": cond = (b1 <= 0 && b2 <= 0 && b3 <= 0 && b4 <= 0); break;
                    }
                    /*
                    if (b1 == 0 && b2 == 0 && b3 == 0 && b4 == 0) {
                        Point3D n = (b - a) % (c - a); n.Normal();
                        if (InsideSolid(s + n * 0.1, p2, p2f) == 1) cond = false;
                    }
                    */
                    Edge e;
                    if (cond) {
                        Face f = new Face(0, 0, 0);
                        b1 = GetIndex(result.Mesh.Vertices, a);
                        if (b1 == -1) {
                            result.Mesh.Vertices.Add(a);
                            f[0] = result.Mesh.Vertices.Count - 1;
                        } else f[0] = b1;
                        b2 = GetIndex(result.Mesh.Vertices, b);
                        if (b2 == -1) {
                            result.Mesh.Vertices.Add(b);
                            f[1] = result.Mesh.Vertices.Count - 1;
                        } else f[1] = b2;
                        b3 = GetIndex(result.Mesh.Vertices, c);
                        if (b3 == -1) {
                            result.Mesh.Vertices.Add(c);
                            f[2] = result.Mesh.Vertices.Count - 1;
                        } else f[2] = b3;
                        if (!FaceExists(result.Mesh.Faces, f)) {
                            result.Mesh.Faces.Add(f);
                            f.Color = p1.Mesh.Faces[i].Color;
                            //Color.FromArgb(rand.Next(255), rand.Next(255), rand.Next(255));
                            e = new Edge(f[0], f[1]);
                            e.Color = p1.Mesh.Edges[0].Color;
                            if (!EdgeExists(result.Mesh.Edges, e)) result.Mesh.Edges.Add(e);
                            e = new Edge(f[0], f[2]);
                            e.Color = p1.Mesh.Edges[0].Color;
                            if (!EdgeExists(result.Mesh.Edges, e)) result.Mesh.Edges.Add(e);
                            e = new Edge(f[1], f[2]);
                            e.Color = p1.Mesh.Edges[0].Color;
                            if (!EdgeExists(result.Mesh.Edges, e)) result.Mesh.Edges.Add(e);
                        }
                    }
                }

                p1.Mesh.Vertices.RemoveRange(p1v, p1.Mesh.Vertices.Count - p1v);
                p1.Mesh.Edges.RemoveRange(p1e, p1.Mesh.Edges.Count - p1e);
                p1.Mesh.Faces.RemoveRange(p1f, p1.Mesh.Faces.Count - p1f);

                // Második menet
                //if (result.Mesh.Vertices.Count == 0) return null; else return result;

                for (int i = 0; i < p2.Mesh.Faces.Count; i++) {
                    if (pf.IsDisposed) return null;
                    int val = 49 + (int)((double)i / (double)p2.Mesh.Faces.Count * 50.0);
                    if (pf.progBar.Value < val) pf.progBar.Value = val;
                    pf.progBar.Refresh();
                    ai = p2.Mesh.Faces[i][0];
                    a = p2.Mesh.Vertices[ai];
                    bi = p2.Mesh.Faces[i][1];
                    b = p2.Mesh.Vertices[bi];
                    ci = p2.Mesh.Faces[i][2];
                    c = p2.Mesh.Vertices[ci];
                    intersect = false;
                    Face.defaultColor = p2.Mesh.Faces[i].Color;
                    Point3D.defaultColor = p2.Mesh.Faces[i].Color;
                    for (int j = 0; j < p1.Mesh.Faces.Count; j++) {
                        // Ha a vizsgált hsz nem metszi a másik test egyetlen lapját sem, az azt jelenti,
                        // hogy teljes egészében kívül vagy belül van a testen.
                        if (TriangleIntersection(a, b, c,
                            p1.Mesh.Vertices[p1.Mesh.Faces[j][0]],
                            p1.Mesh.Vertices[p1.Mesh.Faces[j][1]],
                            p1.Mesh.Vertices[p1.Mesh.Faces[j][2]],
                            out i1, out i2, out oddp)) {
                            // Van metszés, ekkor a vizsgált hsz lapot el kell vágni, létrehozni az új lapokat
                            // és azokat is megvizsgálni, amíg az osztályozás egyértelmű nem lesz.
                            intersect = true;
                            int ind1;
                            p2.Mesh.Vertices.Add(i1);
                            ind1 = p2.Mesh.Vertices.Count - 1;
                            int ind2;
                            p2.Mesh.Vertices.Add(i2);
                            ind2 = p2.Mesh.Vertices.Count - 1;
                            Face f;
                            switch (oddp) {
                                case 1:
                                    if (Equal(i1, b) && Equal(i2, c)) break;
                                    else if (Equal(i1, b)) {
                                        f = new Face(ai, ind1, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(ind1, ci, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                    } else if (Equal(i2, c)) {
                                        f = new Face(ai, ind1, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(ind1, bi, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                    } else {
                                        f = new Face(ai, ind1, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(bi, ind2, ind1);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(ci, ind2, bi);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                    }
                                    break;
                                case 2:
                                    if (Equal(i1, a) && Equal(i2, c)) break;
                                    else if (Equal(i1, a)) {
                                        f = new Face(ind1, bi, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(ind1, ind2, ci);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                    } else if (Equal(i2, c)) {
                                        f = new Face(ind1, bi, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(ind1, ind2, ai);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                    } else {
                                        f = new Face(bi, ind2, ind1);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(ai, ind2, ci);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(ai, ind1, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                    }
                                    break;
                                case 3:
                                    if (Equal(i1, a) && Equal(i2, b)) break;
                                    else if (Equal(i1, a)) {
                                        f = new Face(ci, ind1, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(ind1, bi, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                    } else if (Equal(i2, b)) {
                                        f = new Face(ci, ind1, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(ind1, ai, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                    } else {
                                        f = new Face(ci, ind1, ind2);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(ai, ind2, ind1);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                        f = new Face(bi, ind2, ai);
                                        /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                        p2.Mesh.Faces.Add(f);
                                    }
                                    break;
                                case 10:
                                    if (((b - a) % (c - a)) * ((i1 - a) % (i2 - a)) < -EPS) {
                                        Point3D temp = i1; i1 = i2; i2 = temp;
                                    }
                                    f = new Face(ai, ind1, ind2);
                                    /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                    p2.Mesh.Faces.Add(f);
                                    f = new Face(bi, ci, ind1);
                                    /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                    p2.Mesh.Faces.Add(f);
                                    f = new Face(ci, ind2, ind1);
                                    /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                    p2.Mesh.Faces.Add(f);
                                    break;
                                case 11:
                                    if (((a - b) % (c - b)) * ((i1 - b) % (i2 - b)) < -EPS) {
                                        Point3D temp = i1; i1 = i2; i2 = temp;
                                    }
                                    f = new Face(bi, ind2, ind1);
                                    /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                    p2.Mesh.Faces.Add(f);
                                    f = new Face(ind2, ci, ind1);
                                    /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                    p2.Mesh.Faces.Add(f);
                                    f = new Face(ci, ai, ind1);
                                    /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                    p2.Mesh.Faces.Add(f);
                                    break;
                                case 12:
                                    if (((a - c) % (b - c)) * ((i2 - c) % (i1 - c)) < -EPS) {
                                        Point3D temp = i1; i1 = i2; i2 = temp;
                                    }
                                    f = new Face(ai, ind1, ind2);
                                    /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                    p2.Mesh.Faces.Add(f);
                                    f = new Face(bi, ind1, ai);
                                    /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                    p2.Mesh.Faces.Add(f);
                                    f = new Face(ci, ind2, ind1);
                                    /*if (!FaceExists(p2.Mesh.Faces, f))*/
                                    p2.Mesh.Faces.Add(f);
                                    break;
                            }
                        }
                        if (intersect) break;
                    }
                    if (intersect) continue;
                    // Ha mindhárom pontja kívül van a másik testen, akkor a teljes háromszög 
                    // kívül van a másik testen, így bekerül az eredménybe                    
                    int b1 = InsideSolid(a, p1, p1f);
                    int b2 = InsideSolid(b, p1, p1f);
                    int b3 = InsideSolid(c, p1, p1f);
                    Point3D s = (a + b + c) / 3;
                    int b4 = InsideSolid(s, p1, p1f);
                    bool cond = false;
                    switch (type) {
                        case "union": cond = (b1 <= 0 && b2 <= 0 && b3 <= 0 && b4 < 0); break;
                        case "intersection": cond = (b1 >= 0 && b2 >= 0 && b3 >= 0 && b4 > 0); break;
                        case "difference": cond = (b1 >= 0 && b2 >= 0 && b3 >= 0 && b4 > 0); break;
                    }                     
                    Edge e;
                    if (cond) {
                        if (type == "difference") {
                            Point3D temp = b; b = c; c = temp;
                        }
                        Face f = new Face(0, 0, 0);
                        b1 = GetIndex(result.Mesh.Vertices, a);
                        if (b1 == -1) {
                            result.Mesh.Vertices.Add(a);
                            f[0] = result.Mesh.Vertices.Count - 1;
                        } else f[0] = b1;
                        b2 = GetIndex(result.Mesh.Vertices, b);
                        if (b2 == -1) {
                            result.Mesh.Vertices.Add(b);
                            f[1] = result.Mesh.Vertices.Count - 1;
                        } else f[1] = b2;
                        b3 = GetIndex(result.Mesh.Vertices, c);
                        if (b3 == -1) {
                            result.Mesh.Vertices.Add(c);
                            f[2] = result.Mesh.Vertices.Count - 1;
                        } else f[2] = b3;
                        if (!FaceExists(result.Mesh.Faces, f)) {
                            result.Mesh.Faces.Add(f);
                            f.Color = p2.Mesh.Faces[i].Color;
                              //  Color.FromArgb(rand.Next(255), rand.Next(255), rand.Next(255));
                            e = new Edge(f[0], f[1]);
                            e.Color = p2.Mesh.Edges[0].Color;
                            if (!EdgeExists(result.Mesh.Edges, e)) result.Mesh.Edges.Add(e);
                            e = new Edge(f[0], f[2]);
                            e.Color = p2.Mesh.Edges[0].Color;
                            if (!EdgeExists(result.Mesh.Edges, e)) result.Mesh.Edges.Add(e);
                            e = new Edge(f[1], f[2]);
                            e.Color = p2.Mesh.Edges[0].Color;
                            if (!EdgeExists(result.Mesh.Edges, e)) result.Mesh.Edges.Add(e);
                        }
                    }
                }
                p2.Mesh.Vertices.RemoveRange(p2v, p2.Mesh.Vertices.Count - p2v);
                p2.Mesh.Edges.RemoveRange(p2e, p2.Mesh.Edges.Count - p2e);
                p2.Mesh.Faces.RemoveRange(p2f, p2.Mesh.Faces.Count - p2f);

                if (pf.IsDisposed) return null;
                pf.lCompleted.Text = "Checking consistency...";
                pf.progBar.Value = 0;
                pf.progBar.Maximum = result.Mesh.Vertices.Count;
                pf.progBar.Refresh();
                
                for (int n = 0; n < result.Mesh.Vertices.Count; n++) {
                    if (pf.IsDisposed) return null;
                    pf.progBar.Value++;
                    pf.progBar.Refresh();
                    Point3D p = result.Mesh.Vertices[n];
                    for (int i = 0; i < result.Mesh.Faces.Count; i++) {
                        Face f = result.Mesh.Faces[i];
                        if (f[0] == n || f[1] == n || f[2] == n) continue;
                        bool on = false;
                        int fa = 0, fb = 0, fc = 0;
                        if (Dist_Point_to_Segment(p, result.Mesh.Vertices[f[0]], result.Mesh.Vertices[f[1]]) < EPS) {
                            on = true; fa = f[0]; fb = f[1]; fc = f[2];
                        } else
                            if (Dist_Point_to_Segment(p, result.Mesh.Vertices[f[1]], result.Mesh.Vertices[f[2]]) < EPS) {
                                on = true; fa = f[1]; fb = f[2]; fc = f[0];
                            } else
                                if (Dist_Point_to_Segment(p, result.Mesh.Vertices[f[2]], result.Mesh.Vertices[f[0]]) < EPS) {
                                    on = true; fa = f[2]; fb = f[0]; fc = f[1];
                                }
                        if (on) {
                            Face.defaultColor = f.Color;
                            result.Mesh.Faces.RemoveAt(i);
                            Face f0 = new Face(fa, n, fc);
                            if (!FaceExists(result.Mesh.Faces, f0)) result.Mesh.Faces.Insert(i, f0);
                            f0 = new Face(n, fb, fc);
                            if (!FaceExists(result.Mesh.Faces, f0)) result.Mesh.Faces.Insert(i, f0);
                            i = result.Mesh.Faces.Count;
                        }
                    }
                }
                
                if (result.Mesh.Vertices.Count == 0) return null; else return result;
            }
        }

        private static double Dist_Point_to_Segment(Point3D p, Point3D p0, Point3D p1) {
            Point3D v = p1 - p0;
            Point3D w = p - p0;

            double c1 = w * v;
            if (c1 <= 0) return (p - p0).Norm();

            double c2 = v * v;
            if (c2 <= c1) return (p - p1).Norm();

            double b = c1 / c2;
            Point3D pb = p0 + v * b;
            return (p - pb).Norm();
        }


        private static int GetIndex(List<Point3D> l, Point3D p) {
            for (int i = 0; i < l.Count; i++)
                if (Math.Abs(l[i].X - p.X) < EPS && Math.Abs(l[i].Y - p.Y) < EPS && Math.Abs(l[i].Z - p.Z) < EPS) return i;
            //            if (Math.Abs(l[i].X - p.X) < EPS && Math.Abs(l[i].Y - p.Y) < EPS && Math.Abs(l[i].Z - p.Z) < EPS) return i;
            return -1;
        }

        private static bool FaceExists(List<Face> list, Face f) {
            foreach (Face x in list)
                if (x[0] == f[0] && x[1] == f[1] && x[2] == f[2]) return true;
            return false;
        }

        private static bool EdgeExists(List<Edge> list, Edge e) {
            foreach (Edge x in list)
                if (x.Sp == e.Sp && x.Ep == e.Ep) return true;
            return false;
        }
    }
}
