int PseudoInverse (const Vec3d & col1, const Vec3d & col2, Vec3d & inv1, Vec3d & inv2) { double a11 = col1 * col1; double a12 = col1 * col2; double a22 = col2 * col2; double det = a11 * a22 - a12 * a12; if (fabs (det) < 1e-12 * col1.Length() * col2.Length()) { inv1 = Vec3d (0, 0, 0); inv2 = Vec3d (0, 0, 0); return 1; } double ia11 = a22 / det; double ia12 = -a12 / det; double ia22 = a11 / det; inv1 = ia11 * col1 + ia12 * col2; inv2 = ia12 * col1 + ia22 * col2; return 0; }
int SolveLinearSystemLS2 (const Vec3d & col1, const Vec3d & col2, const Vec2d & rhs, Vec3d & sol, double & x, double & y) { double a11 = col1 * col1; double a12 = col1 * col2; double a22 = col2 * col2; double det = a11 * a22 - a12 * a12; if (fabs (det) <= 1e-12 * col1.Length() * col2.Length() || col1.Length2() == 0 || col2.Length2() == 0) { sol = Vec3d (0, 0, 0); x = 0; y = 0; return 1; } Vec2d invrhs; invrhs.X() = ( a22 * rhs.X() - a12 * rhs.Y()) / det; invrhs.Y() = (-a12 * rhs.X() + a11 * rhs.Y()) / det; sol.X() = invrhs.X() * col1.X() + invrhs.Y() * col2.X(); sol.Y() = invrhs.X() * col1.Y() + invrhs.Y() * col2.Y(); sol.Z() = invrhs.X() * col1.Z() + invrhs.Y() * col2.Z(); x = invrhs.X(); y = invrhs.Y(); return 0; /* Vec3d inv1, inv2; int err = PseudoInverse (col1, col2, inv1, inv2); sol = rhs.X() * inv1 + rhs.Y() * inv2; return err; */ }
void NGInterface::getMeshNormal(int i,double* x) { Element2d &face = ((Mesh*)mesh)->SurfaceElement(i); const Point3d &lp1 = ((Mesh*)mesh)->Point (face.PNum(1)), &lp2 = ((Mesh*)mesh)->Point (face.PNum(2)), &lp3 = ((Mesh*)mesh)->Point (face.PNum(3)); Vec3d n = Cross (Vec3d (lp1, lp2), Vec3d (lp1, lp3)); n /= (n.Length()+1e-12); x[0] = n.X(); x[1] = n.Y(); x[2] = n.Z(); }
void WriteSTLFormat (const Mesh & mesh, const string & filename) { cout << "\nWrite STL Surface Mesh" << endl; ofstream outfile (filename.c_str()); int i, j, k; outfile.precision(10); outfile << "solid" << endl; for (i = 1; i <= mesh.GetNSE(); i++) { outfile << "facet normal "; const Point3d& p1 = mesh.Point(mesh.SurfaceElement(i).PNum(1)); const Point3d& p2 = mesh.Point(mesh.SurfaceElement(i).PNum(2)); const Point3d& p3 = mesh.Point(mesh.SurfaceElement(i).PNum(3)); Vec3d normal = Cross(p2-p1,p3-p1); if (normal.Length() != 0) { normal /= (normal.Length()); } outfile << normal.X() << " " << normal.Y() << " " << normal.Z() << "\n"; outfile << "outer loop\n"; outfile << "vertex " << p1.X() << " " << p1.Y() << " " << p1.Z() << "\n"; outfile << "vertex " << p2.X() << " " << p2.Y() << " " << p2.Z() << "\n"; outfile << "vertex " << p3.X() << " " << p3.Y() << " " << p3.Z() << "\n"; outfile << "endloop\n"; outfile << "endfacet\n"; } outfile << "endsolid" << endl; }
void Vec3d :: GetNormal (Vec3d & n) const { if (fabs (X()) > fabs (Z())) { n.X() = -Y(); n.Y() = X(); n.Z() = 0; } else { n.X() = 0; n.Y() = Z(); n.Z() = -Y(); } double len = n.Length(); if (len == 0) { n.X() = 1; n.Y() = n.Z() = 0; } else n /= len; }
int IntersectTetTriangleRef (const Point<3> ** tri, const int * tripi) { int i, j; double eps = 1e-8; double eps2 = eps * eps; static Point<3> rtetp1(0, 0, 0); static Point<3> rtetp2(1, 0, 0); static Point<3> rtetp3(0, 1, 0); static Point<3> rtetp4(0, 0, 1); static const Point<3> * tet[] = { &rtetp1, &rtetp2, &rtetp3, &rtetp4 }; static int tetpi[] = { 1, 2, 3, 4 }; // return IntersectTetTriangle (tet, tri, tetpi, tripi); int cnt = 0; int tetp1 = -1, tetp2 = -1; int trip1 = -1, trip2 = -1; int tetp3, tetp4, trip3; /* if (!tetpi) { for (i = 0; i <= 2; i++) { for (j = 0; j <= 3; j++) { if (Dist2 (*tet[j], *tri[i]) < eps2) { cnt++; tetp2 = tetp1; tetp1 = j; trip2 = trip1; trip1 = i; break; } } } } else */ { for (i = 0; i <= 2; i++) { for (j = 0; j <= 3; j++) { if (tetpi[j] == tripi[i]) { cnt++; tetp2 = tetp1; tetp1 = j; trip2 = trip1; trip1 = i; break; } } } } // (*testout) << "cnt = " << cnt << endl; switch (cnt) { case 0: { Vec3d no, n; // int inpi[3]; int pside[3][4]; for (j = 0; j < 3; j++) { pside[j][0] = (*tri[j])(0) > -eps; pside[j][1] = (*tri[j])(1) > -eps; pside[j][2] = (*tri[j])(2) > -eps; pside[j][3] = (*tri[j])(0) + (*tri[j])(1) + (*tri[j])(2) < 1+eps; } for (j = 0; j < 4; j++) { if (!pside[0][j] && !pside[1][j] && !pside[2][j]) return 0; } for (j = 0; j < 3; j++) { if (pside[j][0] && pside[j][1] && pside[j][2] && pside[j][3]) return 1; } const Point<3> * line[2], *tetf[3]; for (i = 0; i <= 2; i++) for (j = i+1; j <= 3; j++) { line[0] = tet[i]; line[1] = tet[j]; if (IntersectTriangleLine (tri, &line[0])) return 1; } for (i = 0; i <= 3; i++) { for (j = 0; j <= 2; j++) tetf[j] = tet[(i+j) % 4]; for (j = 0; j <= 2; j++) { line[0] = tri[j]; line[1] = tri[(j+1) % 3]; if (IntersectTriangleLine (&tetf[0], &line[0])) return 1; } } return 0; break; } case 1: { trip2 = 0; if (trip2 == trip1) trip2++; trip3 = 3 - trip1 - trip2; tetp2 = 0; while (tetp2 == tetp1) tetp2++; tetp3 = 0; while (tetp3 == tetp1 || tetp3 == tetp2) tetp3++; tetp4 = 6 - tetp1 - tetp2 - tetp3; Vec3d vtri1 = *tri[trip2] - *tri[trip1]; Vec3d vtri2 = *tri[trip3] - *tri[trip1]; Vec3d ntri; Cross (vtri1, vtri2, ntri); // tri durch tet ? /* Vec3d vtet1(*tet[tetp1], *tet[tetp2]); Vec3d vtet2(*tet[tetp1], *tet[tetp3]); Vec3d vtet3(*tet[tetp1], *tet[tetp4]); Vec3d sol; SolveLinearSystem (vtet1, vtet2, vtet3, vtri1, sol); if (sol.X() > 0 && sol.Y() > 0 && sol.Z() > 0) return 1; SolveLinearSystem (vtet1, vtet2, vtet3, vtri2, sol); if (sol.X() > 0 && sol.Y() > 0 && sol.Z() > 0) return 1; */ // test 3 tet-faces: for (i = 1; i <= 3; i++) { Vec3d vtet1, vtet2; switch (i) { case 1: { vtet1 = *tet[tetp2] - *tet[tetp1]; vtet2 = *tet[tetp3] - *tet[tetp1]; break; } case 2: { vtet1 = *tet[tetp3] - *tet[tetp1]; vtet2 = *tet[tetp4] - *tet[tetp1]; break; } case 3: { vtet1 = *tet[tetp4] - *tet[tetp1]; vtet2 = *tet[tetp2] - *tet[tetp1]; break; } } Vec3d ntet; Cross (vtet1, vtet2, ntet); Vec3d crline = Cross (ntri, ntet); double lcrline = crline.Length(); if (lcrline < eps * eps) continue; if (vtri1 * crline + vtri2 * crline < 0) crline *= -1; double lam1, lam2, lam3, lam4; LocalCoordinates (vtri1, vtri2, crline, lam1, lam2); LocalCoordinates (vtet1, vtet2, crline, lam3, lam4); if (lam1 > -eps && lam2 > -eps && lam3 > -eps && lam4 > -eps) { // (*testout) << "hit, cnt == 1" << "\n"; return 1; } } return 0; break; } case 2: { // common edge tetp3 = 0; while (tetp3 == tetp1 || tetp3 == tetp2) tetp3++; tetp4 = 6 - tetp1 - tetp2 - tetp3; trip3 = 3 - trip1 - trip2; // (*testout) << "trip1,2,3 = " << trip1 << ", " << trip2 << ", " << trip3 << endl; // (*testout) << "tetp1,2,3,4 = " << tetp1 << ", " << tetp2 // << ", " << tetp3 << ", " << tetp4 << endl; Vec3d vtri = *tri[trip3] - *tri[trip1]; Vec3d vtet1 = *tet[tetp3] - *tri[trip1]; Vec3d vtet2 = *tet[tetp4] - *tri[trip1]; Vec3d n = *tri[trip2] - *tri[trip1]; n /= n.Length(); vtet1 -= (n * vtet1) * n; vtet2 -= (n * vtet2) * n; double lam1, lam2; LocalCoordinates (vtet1, vtet2, vtri, lam1, lam2); if (lam1 < -eps || lam2 < -eps) return 0; else { // (*testout) << "vtet1 = " << vtet1 << endl; // (*testout) << "vtet2 = " << vtet2 << endl; // (*testout) << "vtri = " << vtri << endl; // (*testout) << "lam1 = " << lam1 << " lam2 = " << lam2 << endl; // (*testout) << (lam1 * (vtet1 * vtet1) + lam2 * (vtet1 * vtet2)) // << " = " << (vtet1 * vtri) << endl; // (*testout) << (lam1 * (vtet1 * vtet2) + lam2 * (vtet2 * vtet2)) // << " = " << (vtet2 * vtri) << endl; // (*testout) << "tet = "; // for (j = 0; j < 4; j++) // (*testout) << (*tet[j]) << " "; // (*testout) << endl; // (*testout) << "tri = "; // for (j = 0; j < 3; j++) // (*testout) << (*tri[j]) << " "; // (*testout) << endl; // (*testout) << "hit, cnt == 2" << endl; return 1; } break; } case 3: { // common face return 0; } } (*testout) << "hit, cnt = " << cnt << endl; return 1; }
int IntersectTetTriangle (const Point<3> ** tet, const Point<3> ** tri, const int * tetpi, const int * tripi) { int i, j; double diam = Dist (*tri[0], *tri[1]); double epsrel = 1e-8; double eps = diam * epsrel; double eps2 = eps * eps; int cnt = 0; int tetp1 = -1, tetp2 = -1; int trip1 = -1, trip2 = -1; int tetp3, tetp4, trip3; /* for (i = 0; i < 4; i++) loctetpi[i] = -1; */ if (!tetpi) { for (i = 0; i <= 2; i++) { // loctripi[i] = -1; for (j = 0; j <= 3; j++) { if (Dist2 (*tet[j], *tri[i]) < eps2) { // loctripi[i] = j; // loctetpi[j] = i; cnt++; tetp2 = tetp1; tetp1 = j; trip2 = trip1; trip1 = i; break; } } } } else { for (i = 0; i <= 2; i++) { // loctripi[i] = -1; for (j = 0; j <= 3; j++) { if (tetpi[j] == tripi[i]) { // loctripi[i] = j; // loctetpi[j] = i; cnt++; tetp2 = tetp1; tetp1 = j; trip2 = trip1; trip1 = i; break; } } } } // (*testout) << "cnt = " << cnt << endl; // (*testout) << "tet-trig inters, cnt = " << cnt << endl; // cnt .. number of common points switch (cnt) { case 0: { Vec3d no, n; int inpi[3]; // check, if some trigpoint is in tet: for (j = 0; j < 3; j++) inpi[j] = 1; for (i = 1; i <= 4; i++) { int pi1 = i % 4; int pi2 = (i+1) % 4; int pi3 = (i+2) % 4; int pi4 = (i+3) % 4; Vec3d v1 (*tet[pi1], *tet[pi2]); Vec3d v2 (*tet[pi1], *tet[pi3]); Vec3d v3 (*tet[pi1], *tet[pi4]); Cross (v1, v2, n); // n /= n.Length(); double nl = n.Length(); if (v3 * n > 0) n *= -1; int outeri = 1; for (j = 0; j < 3; j++) { Vec3d v(*tet[pi1], *tri[j]); if ( v * n < eps * nl) outeri = 0; else inpi[j] = 0; } if (outeri) return 0; } if (inpi[0] || inpi[1] || inpi[2]) { return 1; } // check, if some tet edge intersects triangle: const Point<3> * line[2], *tetf[3]; for (i = 0; i <= 2; i++) for (j = i+1; j <= 3; j++) { line[0] = tet[i]; line[1] = tet[j]; if (IntersectTriangleLine (tri, &line[0])) return 1; } // check, if triangle line intersects tet face: for (i = 0; i <= 3; i++) { for (j = 0; j <= 2; j++) tetf[j] = tet[(i+j) % 4]; for (j = 0; j <= 2; j++) { line[0] = tri[j]; line[1] = tri[(j+1) % 3]; if (IntersectTriangleLine (&tetf[0], &line[0])) return 1; } } return 0; //GH break; } case 1: { trip2 = 0; while (trip2 == trip1) trip2++; trip3 = 3 - trip1 - trip2; tetp2 = 0; while (tetp2 == tetp1) tetp2++; tetp3 = 0; while (tetp3 == tetp1 || tetp3 == tetp2) tetp3++; tetp4 = 6 - tetp1 - tetp2 - tetp3; Vec3d vtri1 = *tri[trip2] - *tri[trip1]; Vec3d vtri2 = *tri[trip3] - *tri[trip1]; Vec3d ntri; Cross (vtri1, vtri2, ntri); // tri durch tet ? // fehlt noch // test 3 tet-faces: for (i = 1; i <= 3; i++) { Vec3d vtet1, vtet2; switch (i) { case 1: { vtet1 = *tet[tetp2] - *tet[tetp1]; vtet2 = *tet[tetp3] - *tet[tetp1]; break; } case 2: { vtet1 = *tet[tetp3] - *tet[tetp1]; vtet2 = *tet[tetp4] - *tet[tetp1]; break; } case 3: { vtet1 = *tet[tetp4] - *tet[tetp1]; vtet2 = *tet[tetp2] - *tet[tetp1]; break; } } Vec3d ntet; Cross (vtet1, vtet2, ntet); Vec3d crline = Cross (ntri, ntet); double lcrline = crline.Length(); if (lcrline < eps * eps * eps * eps) // new change ! continue; if (vtri1 * crline + vtri2 * crline < 0) crline *= -1; crline /= lcrline; double lam1, lam2, lam3, lam4; LocalCoordinates (vtri1, vtri2, crline, lam1, lam2); LocalCoordinates (vtet1, vtet2, crline, lam3, lam4); if (lam1 > -epsrel && lam2 > -epsrel && lam3 > -epsrel && lam4 > -epsrel) { /* (*testout) << "lcrline = " << lcrline << " eps = " << eps << " diam = " << diam << endl; (*testout) << "hit, cnt == 1 " << "lam1,2,3,4 = " << lam1 << ", " << lam2 << ", " << lam3 << ", " << lam4 << "\n"; */ return 1; } } return 0; //GH break; } case 2: { // common edge tetp3 = 0; while (tetp3 == tetp1 || tetp3 == tetp2) tetp3++; tetp4 = 6 - tetp1 - tetp2 - tetp3; trip3 = 3 - trip1 - trip2; // (*testout) << "trip1,2,3 = " << trip1 << ", " << trip2 << ", " << trip3 << endl; // (*testout) << "tetp1,2,3,4 = " << tetp1 << ", " << tetp2 // << ", " << tetp3 << ", " << tetp4 << endl; Vec3d vtri = *tri[trip3] - *tri[trip1]; Vec3d vtet1 = *tet[tetp3] - *tri[trip1]; Vec3d vtet2 = *tet[tetp4] - *tri[trip1]; Vec3d n = *tri[trip2] - *tri[trip1]; n /= n.Length(); vtet1 -= (n * vtet1) * n; vtet2 -= (n * vtet2) * n; double lam1, lam2; LocalCoordinates (vtet1, vtet2, vtri, lam1, lam2); if (lam1 < -epsrel || lam2 < -epsrel) return 0; else { /* (*testout) << "vtet1 = " << vtet1 << endl; (*testout) << "vtet2 = " << vtet2 << endl; (*testout) << "vtri = " << vtri << endl; (*testout) << "lam1 = " << lam1 << " lam2 = " << lam2 << endl; (*testout) << (lam1 * (vtet1 * vtet1) + lam2 * (vtet1 * vtet2)) << " = " << (vtet1 * vtri) << endl; (*testout) << (lam1 * (vtet1 * vtet2) + lam2 * (vtet2 * vtet2)) << " = " << (vtet2 * vtri) << endl; (*testout) << "tet = "; for (j = 0; j < 4; j++) (*testout) << (*tet[j]) << " "; (*testout) << endl; (*testout) << "tri = "; for (j = 0; j < 3; j++) (*testout) << (*tri[j]) << " "; (*testout) << endl; (*testout) << "hit, cnt == 2" << endl; */ return 1; } break; } case 3: { // common face return 0; } } (*testout) << "hit, cnt = " << cnt << endl; return 1; }