int CalcTriangleCenter (const Point3d ** pts, Point3d & c)
{
static DenseMatrix a(2), inva(2);
static Vector rs(2), sol(2);
double h = Dist(*pts[0], *pts[1]);
Vec3d v1(*pts[0], *pts[1]);
Vec3d v2(*pts[0], *pts[2]);
rs.Elem(1) = v1 * v1;
rs.Elem(2) = v2 * v2;
a.Elem(1,1) = 2 * rs.Get(1);
a.Elem(1,2) = a.Elem(2,1) = 2 * (v1 * v2);
a.Elem(2,2) = 2 * rs.Get(2);
if (fabs (a.Det()) <= 1e-12 * h * h)
{
(*testout) << "CalcTriangleCenter: degenerated" << endl;
return 1;
}
CalcInverse (a, inva);
inva.Mult (rs, sol);
c = *pts[0];
v1 *= sol.Get(1);
v2 *= sol.Get(2);
c += v1;
c += v2;
return 0;
}
void Transformation3d :: CalcInverse (Transformation3d & inv) const
{
static DenseMatrix a(3), inva(3);
static Vector b(3), sol(3);
for (int i = 0; i < 3; i++)
{
b(i) = offset[i];
for (int j = 0; j < 3; j++)
a(i, j) = lin[i][j];
}
::netgen::CalcInverse (a, inva);
inva.Mult (b, sol);
for (int i = 0; i < 3; i++)
{
inv.offset[i] = -sol(i);
for (int j = 0; j < 3; j++)
inv.lin[i][j] = inva(i, j);
}
}
int
IntersectTriangleLine (const Point<3> ** tri, const Point<3> ** line)
{
Vec3d vl(*line[0], *line[1]);
Vec3d vt1(*tri[0], *tri[1]);
Vec3d vt2(*tri[0], *tri[2]);
Vec3d vrs(*tri[0], *line[0]);
static DenseMatrix a(3), ainv(3);
static Vector rs(3), lami(3);
int i;
/*
(*testout) << "Tri-Line inters: " << endl
<< "tri = " << *tri[0] << ", " << *tri[1] << ", " << *tri[2] << endl
<< "line = " << *line[0] << ", " << *line[1] << endl;
*/
for (i = 1; i <= 3; i++)
{
a.Elem(i, 1) = -vl.X(i);
a.Elem(i, 2) = vt1.X(i);
a.Elem(i, 3) = vt2.X(i);
rs.Elem(i) = vrs.X(i);
}
double det = a.Det();
double arel = vl.Length() * vt1.Length() * vt2.Length();
/*
double amax = 0;
for (i = 1; i <= 9; i++)
if (fabs (a.Get(i)) > amax)
amax = fabs(a.Get(i));
*/
// new !!!!
if (fabs (det) <= 1e-10 * arel)
{
#ifdef DEVELOP
// line parallel to triangle !
// cout << "ERROR: IntersectTriangleLine degenerated" << endl;
// (*testout) << "WARNING: IntersectTriangleLine degenerated\n";
/*
(*testout) << "lin-tri intersection: " << endl
<< "line = " << *line[0] << " - " << *line[1] << endl
<< "tri = " << *tri[0] << " - " << *tri[1] << " - " << *tri[2] << endl
<< "lami = " << lami << endl
<< "pc = " << ( *line[0] + lami.Get(1) * vl ) << endl
<< " = " << ( *tri[0] + lami.Get(2) * vt1 + lami.Get(3) * vt2) << endl
<< " a = " << a << endl
<< " ainv = " << ainv << endl
<< " det(a) = " << det << endl
<< " rs = " << rs << endl;
*/
#endif
return 0;
}
CalcInverse (a, ainv);
ainv.Mult (rs, lami);
// (*testout) << "lami = " << lami << endl;
double eps = 1e-6;
if (
(lami.Get(1) >= -eps && lami.Get(1) <= 1+eps &&
lami.Get(2) >= -eps && lami.Get(3) >= -eps &&
lami.Get(2) + lami.Get(3) <= 1+eps) && !
(lami.Get(1) >= eps && lami.Get(1) <= 1-eps &&
lami.Get(2) >= eps && lami.Get(3) >= eps &&
lami.Get(2) + lami.Get(3) <= 1-eps) )
{
#ifdef DEVELOP
// cout << "WARNING: IntersectTriangleLine degenerated" << endl;
(*testout) << "WARNING: IntersectTriangleLine numerical inexact" << endl;
(*testout) << "lin-tri intersection: " << endl
<< "line = " << *line[0] << " - " << *line[1] << endl
<< "tri = " << *tri[0] << " - " << *tri[1] << " - " << *tri[2] << endl
<< "lami = " << lami << endl
<< "pc = " << ( *line[0] + lami.Get(1) * vl ) << endl
<< " = " << ( *tri[0] + lami.Get(2) * vt1 + lami.Get(3) * vt2) << endl
<< " a = " << a << endl
<< " ainv = " << ainv << endl
<< " det(a) = " << det << endl
<< " rs = " << rs << endl;
#endif
}
if (lami.Get(1) >= 0 && lami.Get(1) <= 1 &&
lami.Get(2) >= 0 && lami.Get(3) >= 0 && lami.Get(2) + lami.Get(3) <= 1)
{
return 1;
}
return 0;
}