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