- //cout<<"HasIntersection3"<<endl;
- //cout<<" PC("<<PC.X()<<","<<PC.Y()<<","<<PC.Z()<<")"<<endl;
- //cout<<" P("<<P.X()<<","<<P.Y()<<","<<P.Z()<<")"<<endl;
- //cout<<" P1("<<P1.X()<<","<<P1.Y()<<","<<P1.Z()<<")"<<endl;
- //cout<<" P2("<<P2.X()<<","<<P2.Y()<<","<<P2.Z()<<")"<<endl;
- //cout<<" P3("<<P3.X()<<","<<P3.Y()<<","<<P3.Z()<<")"<<endl;
- gp_Vec VP1(P1,P2);
- gp_Vec VP2(P1,P3);
- IntAna_Quadric IAQ(gp_Pln(P1,VP1.Crossed(VP2)));
- IntAna_IntConicQuad IAICQ(gp_Lin(PC,gp_Dir(gp_Vec(PC,P))),IAQ);
- if(IAICQ.IsDone()) {
- if( IAICQ.IsInQuadric() )
+ const double EPSILON = 1e-6;
+ double segLen = P.Distance( PC );
+
+ gp_XYZ orig = PC.XYZ();
+ gp_XYZ dir = ( P.XYZ() - PC.XYZ() ) / segLen;
+ gp_XYZ vert0 = P1.XYZ();
+ gp_XYZ vert1 = P2.XYZ();
+ gp_XYZ vert2 = P3.XYZ();
+
+ gp_XYZ edge1 = vert1 - vert0;
+ gp_XYZ edge2 = vert2 - vert0;
+
+ /* begin calculating determinant - also used to calculate U parameter */
+ gp_XYZ pvec = dir ^ edge2;
+
+ /* if determinant is near zero, ray lies in plane of triangle */
+ double det = edge1 * pvec;
+
+ const double ANGL_EPSILON = 1e-12;
+ if ( det > -ANGL_EPSILON && det < ANGL_EPSILON )
+ return false;
+
+ /* calculate distance from vert0 to ray origin */
+ gp_XYZ tvec = orig - vert0;
+
+ /* calculate U parameter and test bounds */
+ double u = ( tvec * pvec ) / det;
+ //if (u < 0.0 || u > 1.0)
+ if (u < -EPSILON || u > 1.0 + EPSILON)
+ return false;
+
+ /* prepare to test V parameter */
+ gp_XYZ qvec = tvec ^ edge1;
+
+ /* calculate V parameter and test bounds */
+ double v = (dir * qvec) / det;
+ //if ( v < 0.0 || u + v > 1.0 )
+ if ( v < -EPSILON || u + v > 1.0 + EPSILON)
+ return false;
+
+ /* calculate t, ray intersects triangle */
+ double t = (edge2 * qvec) / det;
+
+ Pint = orig + dir * t;
+
+ bool hasInt = ( t > 0. && t < segLen );
+
+ if ( hasInt && det < EPSILON ) // t is inaccurate, additionally check
+ {
+ gp_XYZ triNorm = edge1 ^ edge2;
+ gp_XYZ int0vec = Pint.XYZ() - vert0;
+ gp_XYZ in = triNorm ^ edge1; // dir inside triangle from edge1
+ double dot = int0vec * in;
+ if ( dot < 0 && dot / triNorm.Modulus() < -EPSILON )
+ return false;
+ in = edge2 ^ triNorm;
+ dot = int0vec * in;
+ if ( dot < 0 && dot / triNorm.Modulus() < -EPSILON )
+ return false;
+ gp_XYZ int1vec = Pint.XYZ() - vert1;
+ in = triNorm ^ ( vert2 - vert1 );
+ dot = int1vec * in;
+ if ( dot < 0 && dot / triNorm.Modulus() < -EPSILON )