From 4f4483794da73c252c074bda715830c5dca3bf0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joachim=20Sch=C3=B6berl?= Date: Thu, 15 Oct 2020 09:29:36 +0200 Subject: [PATCH] VecInSolid, poly --- libsrc/csg/polyhedra.cpp | 1244 ++++++++++++++++++++------------------ libsrc/csg/polyhedra.hpp | 8 + 2 files changed, 657 insertions(+), 595 deletions(-) diff --git a/libsrc/csg/polyhedra.cpp b/libsrc/csg/polyhedra.cpp index 4bb93cdd..c123458b 100644 --- a/libsrc/csg/polyhedra.cpp +++ b/libsrc/csg/polyhedra.cpp @@ -6,322 +6,376 @@ namespace netgen { -Polyhedra::Face::Face (int pi1, int pi2, int pi3, - const NgArray > & points, - int ainputnr) -{ - inputnr = ainputnr; + Polyhedra::Face::Face (int pi1, int pi2, int pi3, + const NgArray > & points, + int ainputnr) + { + inputnr = ainputnr; - pnums[0] = pi1; - pnums[1] = pi2; - pnums[2] = pi3; + pnums[0] = pi1; + pnums[1] = pi2; + pnums[2] = pi3; - bbox.Set (points[pi1]); - bbox.Add (points[pi2]); - bbox.Add (points[pi3]); + bbox.Set (points[pi1]); + bbox.Add (points[pi2]); + bbox.Add (points[pi3]); - v1 = points[pi2] - points[pi1]; - v2 = points[pi3] - points[pi1]; + v1 = points[pi2] - points[pi1]; + v2 = points[pi3] - points[pi1]; - n = Cross (v1, v2); + n = Cross (v1, v2); - nn = n; - nn.Normalize(); - // PseudoInverse (v1, v2, w1, w2); + nn = n; + nn.Normalize(); + // PseudoInverse (v1, v2, w1, w2); - Mat<2,3> mat; - Mat<3,2> inv; - for (int i = 0; i < 3; i++) - { - mat(0,i) = v1(i); - mat(1,i) = v2(i); - } - CalcInverse (mat, inv); - for (int i = 0; i < 3; i++) - { - w1(i) = inv(i,0); - w2(i) = inv(i,1); - } -} + Mat<2,3> mat; + Mat<3,2> inv; + for (int i = 0; i < 3; i++) + { + mat(0,i) = v1(i); + mat(1,i) = v2(i); + } + CalcInverse (mat, inv); + for (int i = 0; i < 3; i++) + { + w1(i) = inv(i,0); + w2(i) = inv(i,1); + } + } -Polyhedra :: Polyhedra () -{ - surfaceactive.SetSize(0); - surfaceids.SetSize(0); - eps_base1 = 1e-8; -} + Polyhedra :: Polyhedra () + { + surfaceactive.SetSize(0); + surfaceids.SetSize(0); + eps_base1 = 1e-8; + } -Polyhedra :: ~Polyhedra () -{ - ; -} + Polyhedra :: ~Polyhedra () + { + ; + } -Primitive * Polyhedra :: CreateDefault () -{ - return new Polyhedra(); -} + Primitive * Polyhedra :: CreateDefault () + { + return new Polyhedra(); + } -INSOLID_TYPE Polyhedra :: BoxInSolid (const BoxSphere<3> & box) const -{ - /* - for (i = 1; i <= faces.Size(); i++) - if (FaceBoxIntersection (i, box)) + INSOLID_TYPE Polyhedra :: BoxInSolid (const BoxSphere<3> & box) const + { + /* + for (i = 1; i <= faces.Size(); i++) + if (FaceBoxIntersection (i, box)) return DOES_INTERSECT; - */ - for (int i = 0; i < faces.Size(); i++) - { - if (!faces[i].bbox.Intersect (box)) - continue; - //(*testout) << "face " << i << endl; + */ + for (int i = 0; i < faces.Size(); i++) + { + if (!faces[i].bbox.Intersect (box)) + continue; + //(*testout) << "face " << i << endl; - const Point<3> & p1 = points[faces[i].pnums[0]]; - const Point<3> & p2 = points[faces[i].pnums[1]]; - const Point<3> & p3 = points[faces[i].pnums[2]]; + const Point<3> & p1 = points[faces[i].pnums[0]]; + const Point<3> & p2 = points[faces[i].pnums[1]]; + const Point<3> & p3 = points[faces[i].pnums[2]]; - if (fabs (faces[i].nn * (p1 - box.Center())) > box.Diam()/2) - continue; + if (fabs (faces[i].nn * (p1 - box.Center())) > box.Diam()/2) + continue; - //(*testout) << "still in loop" << endl; + //(*testout) << "still in loop" << endl; - double dist2 = MinDistTP2 (p1, p2, p3, box.Center()); - //(*testout) << "p1 " << p1 << " p2 " << p2 << " p3 " << p3 << endl - // << " box.Center " << box.Center() << " box.Diam() " << box.Diam() << endl - // << " dist2 " << dist2 << " sqr(box.Diam()/2) " << sqr(box.Diam()/2) << endl; - if (dist2 < sqr (box.Diam()/2)) - { - //(*testout) << "DOES_INTERSECT" << endl; - return DOES_INTERSECT; - } - }; + double dist2 = MinDistTP2 (p1, p2, p3, box.Center()); + //(*testout) << "p1 " << p1 << " p2 " << p2 << " p3 " << p3 << endl + // << " box.Center " << box.Center() << " box.Diam() " << box.Diam() << endl + // << " dist2 " << dist2 << " sqr(box.Diam()/2) " << sqr(box.Diam()/2) << endl; + if (dist2 < sqr (box.Diam()/2)) + { + //(*testout) << "DOES_INTERSECT" << endl; + return DOES_INTERSECT; + } + }; - return PointInSolid (box.Center(), 1e-3 * box.Diam()); -} + return PointInSolid (box.Center(), 1e-3 * box.Diam()); + } // check how many faces a ray starting in p intersects -INSOLID_TYPE Polyhedra :: PointInSolid (const Point<3> & p, - double eps) const -{ - if (!poly_bbox.IsIn (p, eps)) - return IS_OUTSIDE; + INSOLID_TYPE Polyhedra :: PointInSolid (const Point<3> & p, + double eps) const + { + if (!poly_bbox.IsIn (p, eps)) + return IS_OUTSIDE; - // random (?) direction: - Vec<3> n(-0.424621, 0.1543, 0.89212238); + // random (?) direction: + Vec<3> n(-0.424621, 0.1543, 0.89212238); - int cnt = 0; - for (auto & face : faces) - { - Vec<3> v0 = p - points[face.pnums[0]]; + int cnt = 0; + for (auto & face : faces) + { + Vec<3> v0 = p - points[face.pnums[0]]; - double lam3 = face.nn * v0; + double lam3 = face.nn * v0; - if (fabs(lam3) < eps) // point is in plance of face - { - double lam1 = face.w1 * v0; - double lam2 = face.w2 * v0; - if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) - return DOES_INTERSECT; - } - else - { - double lam3 = -(face.n * v0) / (face.n * n); + if (fabs(lam3) < eps) // point is in plance of face + { + double lam1 = face.w1 * v0; + double lam2 = face.w2 * v0; + if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) + return DOES_INTERSECT; + } + else + { + double lam3 = -(face.n * v0) / (face.n * n); - if (lam3 < 0) continue; // ray goes not in direction of face + if (lam3 < 0) continue; // ray goes not in direction of face - Vec<3> rs = v0 + lam3 * n; + Vec<3> rs = v0 + lam3 * n; - double lam1 = face.w1 * rs; - double lam2 = face.w2 * rs; - if (lam1 >= 0 && lam2 >= 0 && lam1+lam2 <= 1) - cnt++; - } - } + double lam1 = face.w1 * rs; + double lam2 = face.w2 * rs; + if (lam1 >= 0 && lam2 >= 0 && lam1+lam2 <= 1) + cnt++; + } + } - return (cnt % 2) ? IS_INSIDE : IS_OUTSIDE; -} + return (cnt % 2) ? IS_INSIDE : IS_OUTSIDE; + } -void Polyhedra :: GetTangentialSurfaceIndices (const Point<3> & p, - NgArray & surfind, double eps) const -{ - for (int i = 0; i < faces.Size(); i++) - { - auto & face = faces[i]; - const Point<3> & p1 = points[face.pnums[0]]; + void Polyhedra :: GetTangentialSurfaceIndices (const Point<3> & p, + NgArray & surfind, double eps) const + { + for (int i = 0; i < faces.Size(); i++) + { + auto & face = faces[i]; + const Point<3> & p1 = points[face.pnums[0]]; - Vec<3> v0 = p - p1; - double lam3 = -(face.nn * v0); // n->nn + Vec<3> v0 = p - p1; + double lam3 = -(face.nn * v0); // n->nn - if (fabs (lam3) > eps) continue; + if (fabs (lam3) > eps) continue; - double lam1 = (face.w1 * v0); - double lam2 = (face.w2 * v0); + double lam1 = (face.w1 * v0); + double lam2 = (face.w2 * v0); - if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) - if (!surfind.Contains (GetSurfaceId(i))) - surfind.Append (GetSurfaceId(i)); - } + if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) + if (!surfind.Contains (GetSurfaceId(i))) + surfind.Append (GetSurfaceId(i)); + } -} + } -#define OLDVECINSOLD -#ifdef OLDVECINSOLD -INSOLID_TYPE Polyhedra :: VecInSolid (const Point<3> & p, - const Vec<3> & v, - double eps) const -{ - NgArray point_on_faces; - INSOLID_TYPE res(DOES_INTERSECT); + INSOLID_TYPE Polyhedra :: VecInSolidOld (const Point<3> & p, + const Vec<3> & v, + double eps) const + { + NgArray point_on_faces; + INSOLID_TYPE res(DOES_INTERSECT); - Vec<3> vn = v; - vn.Normalize(); - for (int i = 0; i < faces.Size(); i++) - { - const Point<3> & p1 = points[faces[i].pnums[0]]; + Vec<3> vn = v; + vn.Normalize(); + for (int i = 0; i < faces.Size(); i++) + { + const Point<3> & p1 = points[faces[i].pnums[0]]; - Vec<3> v0 = p - p1; - double lam3 = -(faces[i].nn * v0); // n->nn + Vec<3> v0 = p - p1; + double lam3 = -(faces[i].nn * v0); // n->nn - if (fabs (lam3) > eps) continue; - //(*testout) << "lam3 <= eps" << endl; + if (fabs (lam3) > eps) continue; + //(*testout) << "lam3 <= eps" << endl; - double lam1 = (faces[i].w1 * v0); - double lam2 = (faces[i].w2 * v0); + double lam1 = (faces[i].w1 * v0); + double lam2 = (faces[i].w2 * v0); - if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) - { - point_on_faces.Append(i); + if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) + { + point_on_faces.Append(i); - double scal = vn * faces[i].nn; // n->nn + double scal = vn * faces[i].nn; // n->nn - res = DOES_INTERSECT; - if (scal > eps_base1) res = IS_OUTSIDE; - if (scal < -eps_base1) res = IS_INSIDE; - } - } + res = DOES_INTERSECT; + if (scal > eps_base1) res = IS_OUTSIDE; + if (scal < -eps_base1) res = IS_INSIDE; + } + } - //(*testout) << "point_on_faces.Size() " << point_on_faces.Size() - // << " res " << res << endl; + //(*testout) << "point_on_faces.Size() " << point_on_faces.Size() + // << " res " << res << endl; + + if (point_on_faces.Size() == 0) + return PointInSolid (p, 0); + if (point_on_faces.Size() == 1) + return res; + + + + + double mindist(0); + bool first = true; + + for(int i=0; i eps && (first || dist < mindist)) + { + mindist = dist; + first = false; + } + } + } + + Point<3> p2 = p + (1e-4*mindist) * vn; + res = PointInSolid (p2, eps); + + // (*testout) << "mindist " << mindist << " res " << res << endl; - if (point_on_faces.Size() == 0) - return PointInSolid (p, 0); - if (point_on_faces.Size() == 1) return res; + } - - double mindist(0); - bool first = true; - - for(int i=0; i eps && (first || dist < mindist)) - { - mindist = dist; - first = false; - } - } - } - - Point<3> p2 = p + (1e-4*mindist) * vn; - res = PointInSolid (p2, eps); - - // (*testout) << "mindist " << mindist << " res " << res << endl; - - return res; -} - -#else - - // check how many faces a ray starting in p+alpha*v intersects -INSOLID_TYPE Polyhedra :: VecInSolid (const Point<3> & p, - const Vec<3> & v, - double eps) const -{ - if (!poly_bbox.IsIn (p, eps)) - return IS_OUTSIDE; + INSOLID_TYPE Polyhedra :: VecInSolidNew (const Point<3> & p, + const Vec<3> & v, + double eps, bool printing) const + { + if (!poly_bbox.IsIn (p, eps)) + return IS_OUTSIDE; - // random (?) direction: - Vec<3> n(-0.424621, 0.1543, 0.89212238); + // random (?) direction: + Vec<3> n(-0.424621, 0.1543, 0.89212238); - int cnt = 0; - for (auto & face : faces) - { - Vec<3> v0 = p - points[face.pnums[0]]; + int cnt = 0; + for (auto & face : faces) + { + Vec<3> v0 = p - points[face.pnums[0]]; + if (printing) + { + *testout << "face: "; + for (int j = 0; j < 3; j++) + *testout << points[face.pnums[j]] << " "; + *testout << endl; + } + double lamn = face.nn * v0; - double lamn = face.nn * v0; + if (fabs(lamn) < eps) // point is in plane of face + { + double lam1 = face.w1 * v0; + double lam2 = face.w2 * v0; + double lam3 = 1-lam1-lam2; + if (printing) + *testout << "lam = " << lam1 << " " << lam2 << " " << lam3 << endl; + if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam3 >= -eps_base1) + { // point is close to trianlge, perturbe by alpha*v + double dlamn = face.nn*v; - if (fabs(lamn) < eps) // point is in plance of face - { - double lam1 = face.w1 * v0; - double lam2 = face.w2 * v0; - double lam3 = 1-lam1-lam2; + if (fabs(dlamn) < 1e-8) // vec also in plane + { + if (printing) + *testout << "tang in plane" << endl; + double dlam1 = face.w1 * v; + double dlam2 = face.w2 * v; + double dlam3 = 1-dlam1-dlam2; + if (printing) + *testout << "dlam = " << dlam1 << " " << dlam2 << " " << dlam3 << endl; + bool in1 = lam1 > eps_base1 || dlam1 > -eps_base1; + bool in2 = lam2 > eps_base1 || dlam2 > -eps_base1; + bool in3 = lam3 > eps_base1 || dlam3 > -eps_base1; + if (in1 && in2 && in3) + return DOES_INTERSECT; + } + else // vec out of plane + { + if (printing) + *testout << "out of plane"; + double dlamn = -(face.n * v) / (face.n * n); + if (printing) + *testout << "dlamn = " << dlamn << endl; + if (dlamn < 0) continue; // ray goes not in direction of face - if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam3 >= -eps_base1) - { // point is close to trianlge, perturbe by alpha*v - double dlamn = face.nn*v; - double dlam1 = face.w1 * v; - double dlam2 = face.w2 * v; - double dlam3 = 1-dlam1-dlam2; + Vec<3> drs = v + dlamn * n; + if (printing) + { + *testout << "drs = " << drs << endl; + *testout << "face.w1 = " << face.w1 << endl; + *testout << "face.w2 = " << face.w2 << endl; + } + + double dlam1 = face.w1 * drs; + double dlam2 = face.w2 * drs; + double dlam3 = -dlam1-dlam2; - if (fabs(dlamn) < 1e-8) // vec also in plane - { - bool in1 = lam1 > eps_base1 || dlam1 > -eps_base1; - bool in2 = lam2 > eps_base1 || dlam2 > -eps_base1; - bool in3 = lam3 > eps_base1 || dlam3 > -eps_base1; - if (in1 && in2 && in3) - return DOES_INTERSECT; - } - else // vec out of plane - { - double dlamn = -(face.n * v) / (face.n * n); - if (dlamn < 0) continue; // ray goes not in direction of face - - Vec<3> drs = v0 + lamn * n; + if (printing) + *testout << "dlam = " << dlam1 << " " << dlam2 << " " << dlam3 << endl; - double dlam1 = dlamn * face.w1 * v; - double dlam2 = dlamn * face.w2 * v; - double dlam3 = 1-dlam1-dlam2; - - bool in1 = lam1 > eps_base1 || dlam1 > -eps_base1; - bool in2 = lam2 > eps_base1 || dlam2 > -eps_base1; - bool in3 = lam3 > eps_base1 || dlam3 > -eps_base1; - - if (in1 && in2 && in3) - cnt++; - } - } - } - else - { - double lamn = -(face.n * v0) / (face.n * n); + bool in1 = lam1 > eps_base1 || dlam1 > -eps_base1; + bool in2 = lam2 > eps_base1 || dlam2 > -eps_base1; + bool in3 = lam3 > eps_base1 || dlam3 > -eps_base1; + + if (in1 && in2 && in3) + { + if (printing) + *testout << "hit" << endl; + cnt++; + } + } + } + } + else + { + double lamn = -(face.n * v0) / (face.n * n); - if (lamn < 0) continue; // ray goes not in direction of face + if (lamn < 0) continue; // ray goes not in direction of face - Vec<3> rs = v0 + lamn * n; + Vec<3> rs = v0 + lamn * n; - double lam1 = face.w1 * rs; - double lam2 = face.w2 * rs; - double lam3 = 1-lam1-lam2; - if (lam1 >= 0 && lam2 >= 0 && lam3 >= 0) - cnt++; - } - } + double lam1 = face.w1 * rs; + double lam2 = face.w2 * rs; + double lam3 = 1-lam1-lam2; + if (lam1 >= 0 && lam2 >= 0 && lam3 >= 0) + { + if (printing) + *testout << "hit" << endl; + cnt++; + } + } + } - return (cnt % 2) ? IS_INSIDE : IS_OUTSIDE; -} + return (cnt % 2) ? IS_INSIDE : IS_OUTSIDE; + } -#endif + + INSOLID_TYPE Polyhedra :: VecInSolid (const Point<3> & p, + const Vec<3> & v, + double eps) const + { + return VecInSolidOld (p, v, eps); + /* + auto oldval = VecInSolidOld (p, v, eps); + auto newval = VecInSolidNew (p, v, eps); + if (oldval != newval) + { + *testout << "different decision: oldval = " << oldval + << " newval = " << newval << endl; + *testout << "p = " << p << ", v = " << v << endl; + VecInSolidNew (p, v, eps, true); + *testout << "Poly:" << endl; + for (auto & face : faces) + { + for (int j = 0; j < 3; j++) + *testout << points[face.pnums[j]] << " "; + *testout << endl; + } + } + return newval; + */ + } @@ -330,28 +384,28 @@ INSOLID_TYPE Polyhedra :: VecInSolid (const Point<3> & p, -/* -INSOLID_TYPE Polyhedra :: VecInSolid2 (const Point<3> & p, - const Vec<3> & v1, - const Vec<3> & v2, - double eps) const -{ - INSOLID_TYPE res; + /* + INSOLID_TYPE Polyhedra :: VecInSolid2 (const Point<3> & p, + const Vec<3> & v1, + const Vec<3> & v2, + double eps) const + { + INSOLID_TYPE res; - res = VecInSolid(p,v1,eps); - if(res != DOES_INTERSECT) - return res; + res = VecInSolid(p,v1,eps); + if(res != DOES_INTERSECT) + return res; - int point_on_n_faces = 0; + int point_on_n_faces = 0; - Vec<3> v1n = v1; - v1n.Normalize(); - Vec<3> v2n = v2; - v2n.Normalize(); + Vec<3> v1n = v1; + v1n.Normalize(); + Vec<3> v2n = v2; + v2n.Normalize(); - for (int i = 0; i < faces.Size(); i++) - { + for (int i = 0; i < faces.Size(); i++) + { const Point<3> & p1 = points[faces[i].pnums[0]]; Vec<3> v0 = p - p1; @@ -363,146 +417,146 @@ INSOLID_TYPE Polyhedra :: VecInSolid2 (const Point<3> & p, double lam2 = (faces[i].w2 * v0); if (lam1 >= -eps && lam2 >= -eps && lam1+lam2 <= 1+eps) - { - double scal1 = v1n * faces[i].n; - if (fabs (scal1) > eps) continue; + { + double scal1 = v1n * faces[i].n; + if (fabs (scal1) > eps) continue; - point_on_n_faces++; + point_on_n_faces++; - double scal2 = v2n * faces[i].n; - res = DOES_INTERSECT; - if (scal2 > eps) res = IS_OUTSIDE; - if (scal2 < -eps) res = IS_INSIDE; - } - } + double scal2 = v2n * faces[i].n; + res = DOES_INTERSECT; + if (scal2 > eps) res = IS_OUTSIDE; + if (scal2 < -eps) res = IS_INSIDE; + } + } - if (point_on_n_faces == 1) - return res; + if (point_on_n_faces == 1) + return res; - cerr << "primitive::vecinsolid2 makes nonsense for polyhedra" << endl; + cerr << "primitive::vecinsolid2 makes nonsense for polyhedra" << endl; - return Primitive :: VecInSolid2 (p, v1, v2, eps); -} -*/ + return Primitive :: VecInSolid2 (p, v1, v2, eps); + } + */ -INSOLID_TYPE Polyhedra :: VecInSolid2 (const Point<3> & p, - const Vec<3> & v1, - const Vec<3> & v2, - double eps) const -{ - //(*testout) << "VecInSolid2 eps " << eps << endl; - INSOLID_TYPE res = VecInSolid(p,v1,eps); - //(*testout) << "VecInSolid = " < & p, + const Vec<3> & v1, + const Vec<3> & v2, + double eps) const + { + //(*testout) << "VecInSolid2 eps " << eps << endl; + INSOLID_TYPE res = VecInSolid(p,v1,eps); + //(*testout) << "VecInSolid = " < v1n = v1; - v1n.Normalize(); - Vec<3> v2n = v2 - (v2 * v1n) * v1n; - v2n.Normalize(); + Vec<3> v1n = v1; + v1n.Normalize(); + Vec<3> v2n = v2 - (v2 * v1n) * v1n; + v2n.Normalize(); - double cosv2, cosv2max = -99; + double cosv2, cosv2max = -99; - for (int i = 0; i < faces.Size(); i++) - { - const Point<3> & p1 = points[faces[i].pnums[0]]; + for (int i = 0; i < faces.Size(); i++) + { + const Point<3> & p1 = points[faces[i].pnums[0]]; - Vec<3> v0 = p - p1; - if (fabs (faces[i].nn * v0) > eps) continue; // n->nn - if (fabs (v1n * faces[i].nn) > eps_base1) continue; // n->nn + Vec<3> v0 = p - p1; + if (fabs (faces[i].nn * v0) > eps) continue; // n->nn + if (fabs (v1n * faces[i].nn) > eps_base1) continue; // n->nn - double lam1 = (faces[i].w1 * v0); - double lam2 = (faces[i].w2 * v0); + double lam1 = (faces[i].w1 * v0); + double lam2 = (faces[i].w2 * v0); - if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) - { - // v1 is in face + if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1) + { + // v1 is in face - Point<3> fc = Center (points[faces[i].pnums[0]], - points[faces[i].pnums[1]], - points[faces[i].pnums[2]]); + Point<3> fc = Center (points[faces[i].pnums[0]], + points[faces[i].pnums[1]], + points[faces[i].pnums[2]]); - Vec<3> vpfc = fc - p; - cosv2 = (v2n * vpfc) / vpfc.Length(); - if (cosv2 > cosv2max) - { - cosv2max = cosv2; - point_on_n_faces++; + Vec<3> vpfc = fc - p; + cosv2 = (v2n * vpfc) / vpfc.Length(); + if (cosv2 > cosv2max) + { + cosv2max = cosv2; + point_on_n_faces++; - double scal2 = v2n * faces[i].nn; // n->nn - res = DOES_INTERSECT; - if (scal2 > eps_base1) res = IS_OUTSIDE; - if (scal2 < -eps_base1) res = IS_INSIDE; + double scal2 = v2n * faces[i].nn; // n->nn + res = DOES_INTERSECT; + if (scal2 > eps_base1) res = IS_OUTSIDE; + if (scal2 < -eps_base1) res = IS_INSIDE; - } - } + } + } + } + + if (point_on_n_faces >= 1) + return res; + + (*testout) << "primitive::vecinsolid2 makes nonsense for polyhedra" << endl; + cerr << "primitive::vecinsolid2 makes nonsense for polyhedra" << endl; + + return Primitive :: VecInSolid2 (p, v1, v2, eps); } - if (point_on_n_faces >= 1) - return res; - - (*testout) << "primitive::vecinsolid2 makes nonsense for polyhedra" << endl; - cerr << "primitive::vecinsolid2 makes nonsense for polyhedra" << endl; - - return Primitive :: VecInSolid2 (p, v1, v2, eps); -} - -void Polyhedra :: GetTangentialVecSurfaceIndices2 (const Point<3> & p, const Vec<3> & v1, const Vec<3> & v2, - NgArray & surfind, double eps) const -{ - Vec<3> v1n = v1; - v1n.Normalize(); - Vec<3> v2n = v2; // - (v2 * v1n) * v1n; - v2n.Normalize(); + void Polyhedra :: GetTangentialVecSurfaceIndices2 (const Point<3> & p, const Vec<3> & v1, const Vec<3> & v2, + NgArray & surfind, double eps) const + { + Vec<3> v1n = v1; + v1n.Normalize(); + Vec<3> v2n = v2; // - (v2 * v1n) * v1n; + v2n.Normalize(); - for (int i = 0; i < faces.Size(); i++) - { - const Point<3> & p1 = points[faces[i].pnums[0]]; + for (int i = 0; i < faces.Size(); i++) + { + const Point<3> & p1 = points[faces[i].pnums[0]]; - Vec<3> v0 = p - p1; - if (fabs (v0 * faces[i].nn) > eps) continue; // n->nn - if (fabs (v1n * faces[i].nn) > eps_base1) continue; // n->nn - if (fabs (v2n * faces[i].nn) > eps_base1) continue; // n->nn + Vec<3> v0 = p - p1; + if (fabs (v0 * faces[i].nn) > eps) continue; // n->nn + if (fabs (v1n * faces[i].nn) > eps_base1) continue; // n->nn + if (fabs (v2n * faces[i].nn) > eps_base1) continue; // n->nn - double lam01 = (faces[i].w1 * v0); - double lam02 = (faces[i].w2 * v0); - double lam03 = 1-lam01-lam02; - double lam11 = (faces[i].w1 * v1); - double lam12 = (faces[i].w2 * v1); - double lam13 = -lam11-lam12; - double lam21 = (faces[i].w1 * v2); - double lam22 = (faces[i].w2 * v2); - double lam23 = -lam21-lam22; + double lam01 = (faces[i].w1 * v0); + double lam02 = (faces[i].w2 * v0); + double lam03 = 1-lam01-lam02; + double lam11 = (faces[i].w1 * v1); + double lam12 = (faces[i].w2 * v1); + double lam13 = -lam11-lam12; + double lam21 = (faces[i].w1 * v2); + double lam22 = (faces[i].w2 * v2); + double lam23 = -lam21-lam22; - bool ok1 = lam01 > eps_base1 || - (lam01 > -eps_base1 && lam11 > eps_base1) || - (lam01 > -eps_base1 && lam11 > -eps_base1 && lam21 > eps_base1); + bool ok1 = lam01 > eps_base1 || + (lam01 > -eps_base1 && lam11 > eps_base1) || + (lam01 > -eps_base1 && lam11 > -eps_base1 && lam21 > eps_base1); - bool ok2 = lam02 > eps_base1 || - (lam02 > -eps_base1 && lam12 > eps_base1) || - (lam02 > -eps_base1 && lam12 > -eps_base1 && lam22 > eps_base1); + bool ok2 = lam02 > eps_base1 || + (lam02 > -eps_base1 && lam12 > eps_base1) || + (lam02 > -eps_base1 && lam12 > -eps_base1 && lam22 > eps_base1); - bool ok3 = lam03 > eps_base1 || - (lam03 > -eps_base1 && lam13 > eps_base1) || - (lam03 > -eps_base1 && lam13 > -eps_base1 && lam23 > eps_base1); + bool ok3 = lam03 > eps_base1 || + (lam03 > -eps_base1 && lam13 > eps_base1) || + (lam03 > -eps_base1 && lam13 > -eps_base1 && lam23 > eps_base1); - if (ok1 && ok2 && ok3) - { - if (!surfind.Contains (GetSurfaceId(faces[i].planenr))) - surfind.Append (GetSurfaceId(faces[i].planenr)); - } - } -} + if (ok1 && ok2 && ok3) + { + if (!surfind.Contains (GetSurfaceId(faces[i].planenr))) + surfind.Append (GetSurfaceId(faces[i].planenr)); + } + } + } @@ -515,101 +569,101 @@ void Polyhedra :: GetTangentialVecSurfaceIndices2 (const Point<3> & p, const Vec -void Polyhedra :: GetPrimitiveData (const char *& classname, - NgArray & coeffs) const -{ - classname = "Polyhedra"; - coeffs.SetSize(0); - coeffs.Append (points.Size()); - coeffs.Append (faces.Size()); - coeffs.Append (planes.Size()); + void Polyhedra :: GetPrimitiveData (const char *& classname, + NgArray & coeffs) const + { + classname = "Polyhedra"; + coeffs.SetSize(0); + coeffs.Append (points.Size()); + coeffs.Append (faces.Size()); + coeffs.Append (planes.Size()); - /* - int i, j; - for (i = 1; i <= planes.Size(); i++) - { + /* + int i, j; + for (i = 1; i <= planes.Size(); i++) + { planes.Elem(i)->Print (*testout); - } - for (i = 1; i <= faces.Size(); i++) - { + } + for (i = 1; i <= faces.Size(); i++) + { (*testout) << "face " << i << " has plane " << faces.Get(i).planenr << endl; for (j = 1; j <= 3; j++) - (*testout) << points.Get(faces.Get(i).pnums[j-1]); + (*testout) << points.Get(faces.Get(i).pnums[j-1]); (*testout) << endl; - } - */ -} + } + */ + } -void Polyhedra :: SetPrimitiveData (NgArray & /* coeffs */) -{ - ; -} + void Polyhedra :: SetPrimitiveData (NgArray & /* coeffs */) + { + ; + } -void Polyhedra :: Reduce (const BoxSphere<3> & box) -{ - for (int i = 0; i < planes.Size(); i++) - surfaceactive[i] = 0; + void Polyhedra :: Reduce (const BoxSphere<3> & box) + { + for (int i = 0; i < planes.Size(); i++) + surfaceactive[i] = 0; - for (int i = 0; i < faces.Size(); i++) - if (FaceBoxIntersection (i, box)) - surfaceactive[faces[i].planenr] = 1; -} + for (int i = 0; i < faces.Size(); i++) + if (FaceBoxIntersection (i, box)) + surfaceactive[faces[i].planenr] = 1; + } -void Polyhedra :: UnReduce () -{ - for (int i = 0; i < planes.Size(); i++) - surfaceactive[i] = 1; -} + void Polyhedra :: UnReduce () + { + for (int i = 0; i < planes.Size(); i++) + surfaceactive[i] = 1; + } -int Polyhedra :: AddPoint (const Point<3> & p) -{ - if(points.Size() == 0) - poly_bbox.Set(p); - else - poly_bbox.Add(p); + int Polyhedra :: AddPoint (const Point<3> & p) + { + if(points.Size() == 0) + poly_bbox.Set(p); + else + poly_bbox.Add(p); - points.Append (p); - return points.Size(); -} + points.Append (p); + return points.Size(); + } -int Polyhedra :: AddFace (int pi1, int pi2, int pi3, int inputnum) -{ - (*testout) << "polyhedra, add face " << pi1 << ", " << pi2 << ", " << pi3 << endl; + int Polyhedra :: AddFace (int pi1, int pi2, int pi3, int inputnum) + { + (*testout) << "polyhedra, add face " << pi1 << ", " << pi2 << ", " << pi3 << endl; - if(pi1 == pi2 || pi2 == pi3 || pi3 == pi1) - { - ostringstream msg; - msg << "Illegal point numbers for polyhedron face: " << pi1+1 << ", " << pi2+1 << ", " << pi3+1; - throw NgException(msg.str()); - } + if(pi1 == pi2 || pi2 == pi3 || pi3 == pi1) + { + ostringstream msg; + msg << "Illegal point numbers for polyhedron face: " << pi1+1 << ", " << pi2+1 << ", " << pi3+1; + throw NgException(msg.str()); + } - faces.Append (Face (pi1, pi2, pi3, points, inputnum)); + faces.Append (Face (pi1, pi2, pi3, points, inputnum)); - Point<3> p1 = points[pi1]; - Point<3> p2 = points[pi2]; - Point<3> p3 = points[pi3]; + Point<3> p1 = points[pi1]; + Point<3> p2 = points[pi2]; + Point<3> p3 = points[pi3]; - Vec<3> v1 = p2 - p1; - Vec<3> v2 = p3 - p1; + Vec<3> v1 = p2 - p1; + Vec<3> v2 = p3 - p1; - Vec<3> n = Cross (v1, v2); - n.Normalize(); + Vec<3> n = Cross (v1, v2); + n.Normalize(); - Plane pl (p1, n); -// int inverse; -// int identicto = -1; -// for (int i = 0; i < planes.Size(); i++) -// if (pl.IsIdentic (*planes[i], inverse, 1e-9*max3(v1.Length(),v2.Length(),Dist(p2,p3)))) -// { -// if (!inverse) -// identicto = i; -// } -// // cout << "is identic = " << identicto << endl; -// identicto = -1; // changed April 10, JS + Plane pl (p1, n); + // int inverse; + // int identicto = -1; + // for (int i = 0; i < planes.Size(); i++) + // if (pl.IsIdentic (*planes[i], inverse, 1e-9*max3(v1.Length(),v2.Length(),Dist(p2,p3)))) + // { + // if (!inverse) + // identicto = i; + // } + // // cout << "is identic = " << identicto << endl; + // identicto = -1; // changed April 10, JS -// if (identicto != -1) -// faces.Last().planenr = identicto; -// else + // if (identicto != -1) + // faces.Last().planenr = identicto; + // else { planes.Append (new Plane (p1, n)); surfaceactive.Append (1); @@ -617,190 +671,190 @@ int Polyhedra :: AddFace (int pi1, int pi2, int pi3, int inputnum) faces.Last().planenr = planes.Size()-1; } -// (*testout) << "is plane nr " << faces.Last().planenr << endl; + // (*testout) << "is plane nr " << faces.Last().planenr << endl; - return faces.Size(); -} + return faces.Size(); + } -int Polyhedra :: FaceBoxIntersection (int fnr, const BoxSphere<3> & box) const -{ - /* - (*testout) << "check face box intersection, fnr = " << fnr << endl; - (*testout) << "box = " << box << endl; - (*testout) << "face-box = " << faces[fnr].bbox << endl; - */ + int Polyhedra :: FaceBoxIntersection (int fnr, const BoxSphere<3> & box) const + { + /* + (*testout) << "check face box intersection, fnr = " << fnr << endl; + (*testout) << "box = " << box << endl; + (*testout) << "face-box = " << faces[fnr].bbox << endl; + */ - if (!faces[fnr].bbox.Intersect (box)) - return 0; + if (!faces[fnr].bbox.Intersect (box)) + return 0; - const Point<3> & p1 = points[faces[fnr].pnums[0]]; - const Point<3> & p2 = points[faces[fnr].pnums[1]]; - const Point<3> & p3 = points[faces[fnr].pnums[2]]; + const Point<3> & p1 = points[faces[fnr].pnums[0]]; + const Point<3> & p2 = points[faces[fnr].pnums[1]]; + const Point<3> & p3 = points[faces[fnr].pnums[2]]; - double dist2 = MinDistTP2 (p1, p2, p3, box.Center()); - /* - (*testout) << "p1 = " << p1 << endl; - (*testout) << "p2 = " << p2 << endl; - (*testout) << "p3 = " << p3 << endl; + double dist2 = MinDistTP2 (p1, p2, p3, box.Center()); + /* + (*testout) << "p1 = " << p1 << endl; + (*testout) << "p2 = " << p2 << endl; + (*testout) << "p3 = " << p3 << endl; - (*testout) << "box.Center() = " << box.Center() << endl; - (*testout) << "center = " << box.Center() << endl; - (*testout) << "dist2 = " << dist2 << endl; - (*testout) << "diam = " << box.Diam() << endl; - */ - if (dist2 < sqr (box.Diam()/2)) - { - // (*testout) << "intersect" << endl; - return 1; - } - return 0; -} - - -void Polyhedra :: GetPolySurfs(NgArray < NgArray * > & polysurfs) -{ - int maxnum = -1; - - for(int i = 0; i maxnum) - maxnum = faces[i].inputnr; - } - - polysurfs.SetSize(maxnum+1); - for(int i=0; i; - - for(int i = 0; iAppend(faces[i].planenr); -} - - -void Polyhedra::CalcSpecialPoints (NgArray > & pts) const -{ - for (int i = 0; i < points.Size(); i++) - pts.Append (points[i]); -} - - -void Polyhedra :: AnalyzeSpecialPoint (const Point<3> & /* pt */, - NgArray > & /* specpts */) const -{ - ; -} - -Vec<3> Polyhedra :: SpecialPointTangentialVector (const Point<3> & p, int s1, int s2) const -{ - const double eps = 1e-10*poly_bbox.Diam(); - - for (int fi1 = 0; fi1 < faces.Size(); fi1++) - for (int fi2 = 0; fi2 < faces.Size(); fi2++) + (*testout) << "box.Center() = " << box.Center() << endl; + (*testout) << "center = " << box.Center() << endl; + (*testout) << "dist2 = " << dist2 << endl; + (*testout) << "diam = " << box.Diam() << endl; + */ + if (dist2 < sqr (box.Diam()/2)) { - int si1 = faces[fi1].planenr; - int si2 = faces[fi2].planenr; + // (*testout) << "intersect" << endl; + return 1; + } + return 0; + } - if (surfaceids[si1] != s1 || surfaceids[si2] != s2) continue; - //(*testout) << "check pair fi1/fi2 " << fi1 << "/" << fi2 << endl; + void Polyhedra :: GetPolySurfs(NgArray < NgArray * > & polysurfs) + { + int maxnum = -1; + + for(int i = 0; i maxnum) + maxnum = faces[i].inputnr; + } + + polysurfs.SetSize(maxnum+1); + for(int i=0; i; + + for(int i = 0; iAppend(faces[i].planenr); + } + + + void Polyhedra::CalcSpecialPoints (NgArray > & pts) const + { + for (int i = 0; i < points.Size(); i++) + pts.Append (points[i]); + } + + + void Polyhedra :: AnalyzeSpecialPoint (const Point<3> & /* pt */, + NgArray > & /* specpts */) const + { + ; + } + + Vec<3> Polyhedra :: SpecialPointTangentialVector (const Point<3> & p, int s1, int s2) const + { + const double eps = 1e-10*poly_bbox.Diam(); + + for (int fi1 = 0; fi1 < faces.Size(); fi1++) + for (int fi2 = 0; fi2 < faces.Size(); fi2++) + { + int si1 = faces[fi1].planenr; + int si2 = faces[fi2].planenr; + + if (surfaceids[si1] != s1 || surfaceids[si2] != s2) continue; + + //(*testout) << "check pair fi1/fi2 " << fi1 << "/" << fi2 << endl; - Vec<3> n1 = GetSurface(si1) . GetNormalVector (p); - Vec<3> n2 = GetSurface(si2) . GetNormalVector (p); - Vec<3> t = Cross (n1, n2); + Vec<3> n1 = GetSurface(si1) . GetNormalVector (p); + Vec<3> n2 = GetSurface(si2) . GetNormalVector (p); + Vec<3> t = Cross (n1, n2); - //(*testout) << "t = " << t << endl; + //(*testout) << "t = " << t << endl; - /* - int samepts = 0; - for (int j = 0; j < 3; j++) - for (int k = 0; k < 3; k++) + /* + int samepts = 0; + for (int j = 0; j < 3; j++) + for (int k = 0; k < 3; k++) if (Dist(points[faces[fi1].pnums[j]], - points[faces[fi2].pnums[k]]) < eps) - samepts++; - if (samepts < 2) continue; - */ + points[faces[fi2].pnums[k]]) < eps) + samepts++; + if (samepts < 2) continue; + */ - bool shareedge = false; - for(int j = 0; !shareedge && j < 3; j++) - { - Vec<3> v1 = points[faces[fi1].pnums[(j+1)%3]] - points[faces[fi1].pnums[j]]; - double smax = v1.Length(); - v1 *= 1./smax; + bool shareedge = false; + for(int j = 0; !shareedge && j < 3; j++) + { + Vec<3> v1 = points[faces[fi1].pnums[(j+1)%3]] - points[faces[fi1].pnums[j]]; + double smax = v1.Length(); + v1 *= 1./smax; - int pospos; - if(fabs(v1(0)) > 0.5) - pospos = 0; - else if(fabs(v1(1)) > 0.5) - pospos = 1; - else - pospos = 2; + int pospos; + if(fabs(v1(0)) > 0.5) + pospos = 0; + else if(fabs(v1(1)) > 0.5) + pospos = 1; + else + pospos = 2; - double sp = (p(pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos); - if(sp < -eps || sp > smax+eps) - continue; + double sp = (p(pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos); + if(sp < -eps || sp > smax+eps) + continue; - for (int k = 0; !shareedge && k < 3; k ++) - { - Vec<3> v2 = points[faces[fi2].pnums[(k+1)%3]] - points[faces[fi2].pnums[k]]; - v2.Normalize(); - if(v2 * v1 > 0) - v2 -= v1; - else - v2 += v1; + for (int k = 0; !shareedge && k < 3; k ++) + { + Vec<3> v2 = points[faces[fi2].pnums[(k+1)%3]] - points[faces[fi2].pnums[k]]; + v2.Normalize(); + if(v2 * v1 > 0) + v2 -= v1; + else + v2 += v1; - //(*testout) << "v2.Length2() " << v2.Length2() << endl; + //(*testout) << "v2.Length2() " << v2.Length2() << endl; - if(v2.Length2() > 1e-18) - continue; + if(v2.Length2() > 1e-18) + continue; - double sa,sb; + double sa,sb; - sa = (points[faces[fi2].pnums[k]](pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos); - sb = (points[faces[fi2].pnums[(k+1)%3]](pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos); + sa = (points[faces[fi2].pnums[k]](pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos); + sb = (points[faces[fi2].pnums[(k+1)%3]](pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos); - if(Dist(points[faces[fi1].pnums[j]] + sa*v1, points[faces[fi2].pnums[k]]) > eps) - continue; + if(Dist(points[faces[fi1].pnums[j]] + sa*v1, points[faces[fi2].pnums[k]]) > eps) + continue; - if(sa > sb) - { - double aux = sa; sa = sb; sb = aux; - } + if(sa > sb) + { + double aux = sa; sa = sb; sb = aux; + } - //testout->precision(20); - //(*testout) << "sa " << sa << " sb " << sb << " smax " << smax << " sp " << sp << " v1 " << v1 << endl; - //testout->precision(8); + //testout->precision(20); + //(*testout) << "sa " << sa << " sb " << sb << " smax " << smax << " sp " << sp << " v1 " << v1 << endl; + //testout->precision(8); - shareedge = (sa < -eps && sb > eps) || - (sa < smax-eps && sb > smax+eps) || - (sa > -eps && sb < smax+eps); + shareedge = (sa < -eps && sb > eps) || + (sa < smax-eps && sb > smax+eps) || + (sa > -eps && sb < smax+eps); - if(!shareedge) - continue; + if(!shareedge) + continue; - sa = max2(sa,0.); - sb = min2(sb,smax); + sa = max2(sa,0.); + sb = min2(sb,smax); - if(sp < sa+eps) - shareedge = (t * v1 > 0); - else if (sp > sb-eps) - shareedge = (t * v1 < 0); + if(sp < sa+eps) + shareedge = (t * v1 > 0); + else if (sp > sb-eps) + shareedge = (t * v1 < 0); - } - } - if (!shareedge) continue; + } + } + if (!shareedge) continue; - t.Normalize(); + t.Normalize(); - return t; - } + return t; + } - return Vec<3> (0,0,0); -} + return Vec<3> (0,0,0); + } } diff --git a/libsrc/csg/polyhedra.hpp b/libsrc/csg/polyhedra.hpp index f7a14cd4..720786e4 100644 --- a/libsrc/csg/polyhedra.hpp +++ b/libsrc/csg/polyhedra.hpp @@ -54,10 +54,18 @@ namespace netgen virtual INSOLID_TYPE BoxInSolid (const BoxSphere<3> & box) const; virtual INSOLID_TYPE PointInSolid (const Point<3> & p, double eps) const; + virtual INSOLID_TYPE VecInSolidNew (const Point<3> & p, + const Vec<3> & v, + double eps, bool printing = false) const; + virtual INSOLID_TYPE VecInSolidOld (const Point<3> & p, + const Vec<3> & v, + double eps) const; virtual INSOLID_TYPE VecInSolid (const Point<3> & p, const Vec<3> & v, double eps) const; + + // checks if lim s->0 lim t->0 p + t(v1 + s v2) in solid virtual INSOLID_TYPE VecInSolid2 (const Point<3> & p, const Vec<3> & v1,