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;
}
Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d &source)
{
if (source.size() < 3)
return NULL;
if (source.size() % 3 != 0)
logError("The number of vertices to construct a mesh from is not divisible by 3. Probably constructed triangles will not make sense.");
std::set<detail::LocalVertexType, detail::ltLocalVertexValue> vertices;
std::vector<unsigned int> triangles;
unsigned int n = source.size() / 3;
for (unsigned int i = 0 ; i < n ; ++i)
{
// check if we have new vertices
unsigned int i3 = i * 3;
detail::LocalVertexType vt1(source[i3]);
std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p1 = vertices.find(vt1);
if (p1 == vertices.end())
{
vt1.index = vertices.size();
vertices.insert(vt1);
}
else
vt1.index = p1->index;
triangles.push_back(vt1.index);
detail::LocalVertexType vt2(source[++i3]);
std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p2 = vertices.find(vt2);
if (p2 == vertices.end())
{
vt2.index = vertices.size();
vertices.insert(vt2);
}
else
vt2.index = p2->index;
triangles.push_back(vt2.index);
detail::LocalVertexType vt3(source[++i3]);
std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p3 = vertices.find(vt3);
if (p3 == vertices.end())
{
vt3.index = vertices.size();
vertices.insert(vt3);
}
else
vt3.index = p3->index;
triangles.push_back(vt3.index);
}
// sort our vertices
std::vector<detail::LocalVertexType> vt;
vt.insert(vt.end(), vertices.begin(), vertices.end());
std::sort(vt.begin(), vt.end(), detail::ltLocalVertexIndex());
// copy the data to a mesh structure
unsigned int nt = triangles.size() / 3;
Mesh *mesh = new Mesh(vt.size(), nt);
for (unsigned int i = 0 ; i < vt.size() ; ++i)
{
unsigned int i3 = i * 3;
mesh->vertices[i3 ] = vt[i].x;
mesh->vertices[i3 + 1] = vt[i].y;
mesh->vertices[i3 + 2] = vt[i].z;
}
std::copy(triangles.begin(), triangles.end(), mesh->triangles);
mesh->computeNormals();
return mesh;
}
// static
void HDimension::draw_arrow_line(DimensionMode mode, const gp_Pnt &p0, const gp_Pnt &p1, const gp_Pnt &p2, const gp_Dir &xdir, const gp_Dir &ydir, double width, double scale)
{
double short_line_length = 5.0 * scale;
double long_line_extra = 2.0 * scale;
double y0 = gp_Vec(p2.XYZ()) * gp_Vec(ydir.XYZ()) - gp_Vec(p0.XYZ()) * gp_Vec(ydir.XYZ());
double y1 = gp_Vec(p2.XYZ()) * gp_Vec(ydir.XYZ()) - gp_Vec(p1.XYZ()) * gp_Vec(ydir.XYZ());
gp_Pnt vt0( p0.XYZ() + ydir.XYZ() * y0);
gp_Pnt vt1( p1.XYZ() + ydir.XYZ() * y1);
gp_Pnt vt2 = p2;
gp_Dir along_dir = make_vector(gp_Pnt(p0), gp_Pnt(p1));
gp_Dir xdir_along = xdir;
if(along_dir * xdir < 0)xdir_along = -xdir;
gp_Pnt new_vt0 = vt0;
gp_Pnt new_vt1 = vt1;
gp_Pnt middle_text_point = p2.XYZ() + along_dir.XYZ() * (width/2 * scale);
double x0 = gp_Vec(p0.XYZ()) * gp_Vec(xdir_along.XYZ());
double x1 = gp_Vec(p1.XYZ()) * gp_Vec(xdir_along.XYZ());
double xm = gp_Vec(middle_text_point.XYZ()) * gp_Vec(xdir_along.XYZ());
double arrow_head_scale = scale;
if(xm < x0 || xm > x1)
{
arrow_head_scale *= -1;
}
double distance = vt0.Distance(vt1);
// draw arrow heads, if there's room
if((distance > 2 * scale + wxGetApp().m_geom_tol) || (xm < x0) || (xm > x1))
{
gp_XYZ t[2][3];
t[0][0] = vt0.XYZ();
t[0][1] = vt0.XYZ() + xdir_along.XYZ() * arrow_head_scale + ydir.XYZ() * (arrow_head_scale * (-0.4));
t[0][2] = vt0.XYZ() + xdir_along.XYZ() * arrow_head_scale + ydir.XYZ() * (arrow_head_scale * 0.4);
t[1][0] = vt1.XYZ();
t[1][1] = vt1.XYZ() + xdir_along.XYZ() * (-arrow_head_scale) + ydir.XYZ() * (arrow_head_scale * 0.4);
t[1][2] = vt1.XYZ() + xdir_along.XYZ() * (-arrow_head_scale) + ydir.XYZ() * (arrow_head_scale * (-0.4));
// adjust line vertices
new_vt0 = gp_Pnt(vt0.XYZ() + xdir_along.XYZ() * arrow_head_scale);
new_vt1 = gp_Pnt(vt1.XYZ() + xdir_along.XYZ() * (-arrow_head_scale));
// draw two triangles
for(int i = 0; i<2; i++)
{
glBegin(GL_LINE_STRIP);
glVertex3d(t[i][0].X(), t[i][0].Y(), t[i][0].Z());
glVertex3d(t[i][1].X(), t[i][1].Y(), t[i][1].Z());
glVertex3d(t[i][2].X(), t[i][2].Y(), t[i][2].Z());
glVertex3d(t[i][0].X(), t[i][0].Y(), t[i][0].Z());
glEnd();
}
}
// draw side lines
glBegin(GL_LINES);
glVertex3d(p0.X(), p0.Y(), p0.Z());
glVertex3d(vt0.X(), vt0.Y(), vt0.Z());
glVertex3d(p1.X(), p1.Y(), p1.Z());
glVertex3d(vt1.X(), vt1.Y(), vt1.Z());
glEnd();
if(xm < x0)
{
// long line first
gp_Pnt vt4 = vt2.XYZ() + xdir_along.XYZ() * (-long_line_extra);
glBegin(GL_LINES);
glVertex3d(vt2.X(), vt2.Y(), vt2.Z());
glVertex3d(new_vt0.X(), new_vt0.Y(), new_vt0.Z());
glEnd();
// little line
gp_Pnt vt3 = new_vt1.XYZ() + xdir_along.XYZ() * short_line_length;
glBegin(GL_LINES);
glVertex3d(new_vt1.X(), new_vt1.Y(), new_vt1.Z());
glVertex3d(vt3.X(), vt3.Y(), vt3.Z());
glEnd();
}
else if(xm > x1)
{
// little first
gp_Pnt vt3 = new_vt0.XYZ() - xdir_along.XYZ() * short_line_length;
glBegin(GL_LINES);
glVertex3d(vt3.X(), vt3.Y(), vt3.Z());
glVertex3d(new_vt0.X(), new_vt0.Y(), new_vt0.Z());
glEnd();
// long line
glBegin(GL_LINES);
gp_Pnt vt4 = vt2.XYZ() + xdir_along.XYZ() * (width * scale + long_line_extra);
glVertex3d(vt1.X(), vt1.Y(), vt1.Z());
glVertex3d(vt4.X(), vt4.Y(), vt4.Z());
glEnd();
}
else
{
// draw the arrow line
glBegin(GL_LINES);
glVertex3d(new_vt0.X(), new_vt0.Y(), new_vt0.Z());
glVertex3d(new_vt1.X(), new_vt1.Y(), new_vt1.Z());
glEnd();
}
}
