cgma
IntersectionTool.cpp
Go to the documentation of this file.
00001 // file: IntersectionTool.cpp
00002 // author: Michael Stephenson
00003 //
00004 
00005 #include <math.h>
00006 #include "IntersectionTool.hpp"
00007 #include "CubitVector.hpp"
00008 #include "CubitMessage.hpp"
00009 #include "GeometryDefines.h"
00010 #include "DLIList.hpp"
00011 
00012 
00013   
00014 // double IntersectionTool::distance_point_line(const double point[3], 
00015 //                                              const double start[3],
00016 //                                              const double end[3], 
00017 //                                              double &t)
00018 // {
00019 //   int i;
00020 //   double distSq = 0.0;
00021 //   for (i = 0; i < 3; i++)
00022 //     distSq += (point[i] - end[i]) * (point[i] - end[i]);
00023 //   if (distSq < mTolerance) {
00024 //     t = 1.0;
00025 //     return 0.0;
00026 //   }
00027 
00028 //   distSq = 0.0;
00029 //   for (i = 0; i < 3; i++)
00030 //     distSq += (point[i] - start[i]) * (point[i] - start[i]);
00031 //   if (distSq < mTolerance) {
00032 //     t = 0.0;
00033 //     return 0.0;
00034 //   }
00035 
00036 //   double div = 0.0;
00037 //   for (i = 0; i < 3; i++)
00038 //     div += (end[i] - start[i]) * (end[i] - start[i]);
00039 //   if (div < mTolerance) {
00040 //     return -1.0;
00041 //   }
00042 
00043 //   t = sqrt(distSq)/div;
00044   
00045 //   double pt[3];
00046 //   for (i = 0; i < 3; i++)
00047 //     pt[i] = (end[i] - start[i]) * t + start[i];
00048 
00049 //   distSq = 0;
00050 //   for (i = 0; i < 3; i++)
00051 //     distSq += (point[i] - pt[i]) * (point[i] - pt[i]);
00052   
00053 //   return sqrt(distSq);
00054 // }
00055 
00056 double IntersectionTool::parametric_position(const double node[3],
00057                                            const double pt1[3],
00058                                            const double pt2[3])
00059 {
00060   int i;
00061   double dist_sq, t;
00062   
00063     // check for end-points
00064   double p13[3];
00065   for (i = 0; i < 3; i++)
00066     p13[i] = node[i] - pt1[i];
00067   dist_sq = p13[0] * p13[0] +  p13[1] * p13[1] +  p13[2] * p13[2];
00068   if (dist_sq < mToleranceSquared) {
00069     t = 0.0;
00070     return t;
00071   }
00072 
00073   double p23[3];
00074   for (i = 0; i < 3; i++)
00075     p23[i] = node[i] - pt2[i];
00076   dist_sq = p23[0] * p23[0] +  p23[1] * p23[1] +  p23[2] * p23[2];
00077   if (dist_sq < mToleranceSquared) {
00078     t = 1.0;
00079     return t;
00080   }
00081   
00082     // t is parametric distance along vector p12
00083   double p12[3];
00084   for (i = 0; i < 3; i++)
00085     p12[i] = pt2[i] - pt1[i];
00086   
00087     // point1 and point2 are coincident if dot1 is zero
00088   double dot1 = p12[0] * p12[0] + p12[1] * p12[1] + p12[2] * p12[2];
00089   if (dot1 > -mToleranceSquared && dot1 < mToleranceSquared)
00090     return CUBIT_DBL_MAX;
00091   t = (p13[0] * p12[0] + p13[1] * p12[1] + p13[2] * p12[2]) / dot1;
00092   if (t > -mTolerance && t <  mTolerance)
00093     t = 0.0;
00094   else if ((t - 1.0) > -mTolerance &&
00095            (t - 1.0) <  mTolerance)
00096     t = 1.0;
00097   return t;
00098 }
00099 
00100 int IntersectionTool::point_on_polyline(CubitVector& pt, DLIList<CubitVector*> &pt_list,
00101                                         double *tol_in)
00102 {
00103   int i, ret;
00104   double t, distance;
00105   double pt_coords[3];
00106   double line_coords1[3];
00107   double line_coords2[3];
00108   double tol = GEOMETRY_RESABS;
00109 
00110   if(tol_in)
00111     tol = *tol_in;
00112 
00113   ret = 0;
00114 
00115   pt_coords[0] = pt.x();
00116   pt_coords[1] = pt.y();
00117   pt_coords[2] = pt.z();
00118 
00119   pt_list.reset();
00120   CubitVector *last_pt = pt_list.get_and_step();
00121   for(i=pt_list.size(); i>1; i--)
00122   {
00123     CubitVector *next_pt = pt_list.get_and_step();
00124 
00125     line_coords1[0] = last_pt->x();
00126     line_coords1[1] = last_pt->y();
00127     line_coords1[2] = last_pt->z();
00128     line_coords2[0] = next_pt->x();
00129     line_coords2[1] = next_pt->y();
00130     line_coords2[2] = next_pt->z();
00131 
00132     distance = distance_point_line(pt_coords, line_coords1, line_coords2, t);
00133 
00134     if(distance > -tol && distance < tol)
00135     {
00136       i = 1;
00137       ret = 1;
00138     }
00139     else
00140       last_pt = next_pt;
00141   }
00142   return ret;
00143 }
00144 
00145 double IntersectionTool::distance_point_line(const double node[3], 
00146                                              const double pt1[3],
00147                                              const double pt2[3], 
00148                                              double &t)
00149 {
00150   double dist_sq;
00151   int i;
00152   double p12[3];
00153   for (i = 0; i < 3; i++)
00154     p12[i] = pt2[i] - pt1[i];
00155   
00156   t = parametric_position(node, pt1, pt2);
00157   if ( t == CUBIT_DBL_MAX )
00158     return -1.;
00159   //if ( t== 0.0 || t == 1.0)
00160   //  return 0.0;
00161     // is t on vector p12 or its infinite extension
00162   if (t < 0.0 || t > 1.0)
00163     return -1.0;
00164     // calculate point on p12
00165   double p[3];
00166   for (i = 0; i < 3; i++)
00167     p[i] = pt1[i] + t * p12[i];
00168     
00169     // return distance from node to point on p12
00170   dist_sq = 0.0;
00171   for (i = 0; i < 3; i++)
00172     dist_sq += (p[i] - node[i]) * (p[i] - node[i]);
00173 
00174   return sqrt(dist_sq);
00175 }
00176 
00177 CubitBoolean IntersectionTool::ray_tri_test(const double start[3],
00178                                             const double dir[3],
00179                                             const double vert0[3],
00180                                             const double vert1[3], 
00181                                             const double vert2[3],
00182                                             double &t, double &u, double &v)
00183 {
00184   int i;
00185   
00186     // find vectors for two edges sharing vert0
00187   double edge1[3];
00188   double edge2[3];
00189   for (i = 0; i < 3; i++) {
00190     edge1[i] = vert1[i] - vert0[i];
00191     edge2[i] = vert2[i] - vert0[i];
00192   }
00193   
00194     // calculate determinate
00195   double pvec[3];
00196   pvec[0] = dir[1] * edge2[2] - dir[2] * edge2[1];
00197   pvec[1] = dir[2] * edge2[0] - dir[0] * edge2[2];
00198   pvec[2] = dir[0] * edge2[1] - dir[1] * edge2[0];
00199   double det =
00200     edge1[0] * pvec[0] + edge1[1] * pvec[1] + edge1[2] * pvec[2];
00201   
00202     // if determinate is near zero, the ray is in plane of triangle
00203   if (det > -mTolerance && det < mTolerance) {
00204     return CUBIT_FALSE;
00205   }
00206   
00207     // calculate distance from vert0 to ray origin
00208   double tvec[3];
00209   for (i = 0; i < 3; i++)
00210     tvec[i] = start[i] - vert0[i];
00211   
00212     // calculate U parameter and test bounds
00213   double inv_det = 1.0/det;
00214   u = (tvec[0] * pvec[0] + tvec[1] * pvec[1] + tvec[2] * pvec[2]) * inv_det;
00215   if (u > -mTolerance && u < mTolerance) u = 0.0;
00216   else if ((u - 1.0) > -mTolerance &&
00217            (u - 1.0) <  mTolerance) u = 1.0;
00218   if (u < 0.0 || u > 1.0) {
00219     return CUBIT_FALSE;
00220   }
00221 
00222     // calculate V parameter and test bounds
00223   double qvec[3];
00224   qvec[0] = tvec[1] * edge1[2] - tvec[2] * edge1[1];
00225   qvec[1] = tvec[2] * edge1[0] - tvec[0] * edge1[2];
00226   qvec[2] = tvec[0] * edge1[1] - tvec[1] * edge1[0];
00227   v = (dir[0] * qvec[0] + dir[1] * qvec[1] + dir[2] * qvec[2]) * inv_det;
00228   if (v > -mTolerance && v < mTolerance) v = 0.0;
00229   else if ((v - 1.0) > -mTolerance &&
00230            (v - 1.0) <  mTolerance) v = 1.0;
00231   if (v < 0.0 || (u + v - 1.0) > mTolerance) {    
00232     return CUBIT_FALSE;
00233   }
00234 
00235     // calculate T, ray intersects triangle
00236   t = (edge2[0] * qvec[0] + edge2[1] * qvec[1] + 
00237        edge2[2] * qvec[2]) * inv_det;
00238   if (t > -mTolerance && t < mTolerance) t = 0.0;
00239   else if ((t - 1.0) > -mTolerance &&
00240            (t - 1.0) <  mTolerance) t = 1.0;
00241 
00242   return CUBIT_TRUE;
00243 }
00244 
00245 
00246 CubitBoolean IntersectionTool::skew_line_test(const double start1[3], 
00247                                               const double end1[3],
00248                                               const double start2[3], 
00249                                               const double end2[3],
00250                                               double &t, double &u)
00251 {
00252   t = -1.0;
00253   u = -1.0;
00254   
00255   int i;
00256   double p13[3], p43[3];
00257   for (i = 0; i < 3; i++) {
00258     p13[i] = start1[i] - start2[i];
00259     p43[i] = end2[i]   - start2[i];
00260   }
00261   double len_sq1 = p43[0] * p43[0] + p43[1] * p43[1] + p43[2] * p43[2];
00262   if (len_sq1 < mToleranceSquared)
00263     return CUBIT_FALSE;
00264 
00265   double p21[3];
00266   for (i = 0; i < 3; i++) {
00267     p21[i] = end1[i] - start1[i];
00268   }
00269   double len_sq2 = p21[0] * p21[0] + p21[1] * p21[1] + p21[2] * p21[2];
00270   if (len_sq2 < mToleranceSquared)
00271     return CUBIT_FALSE;
00272 
00273   double fact;
00274   if (len_sq2 < len_sq1) fact = 10.0/sqrt(len_sq1);
00275   else                   fact = 10.0/sqrt(len_sq2);
00276   for (i = 0; i < 3; i++) {
00277     p13[i] *= fact;
00278     p43[i] *= fact;
00279     p21[i] *= fact;
00280   }
00281 
00282   double d1343, d4321, d1321, d4343, d2121;
00283   d1343 = p13[0] * p43[0] + p13[1] * p43[1] + p13[2] * p43[2];
00284   d4321 = p43[0] * p21[0] + p43[1] * p21[1] + p43[2] * p21[2];
00285   d1321 = p13[0] * p21[0] + p13[1] * p21[1] + p13[2] * p21[2];
00286   d4343 = p43[0] * p43[0] + p43[1] * p43[1] + p43[2] * p43[2];
00287   d2121 = p21[0] * p21[0] + p21[1] * p21[1] + p21[2] * p21[2];
00288 
00289   double denom = d2121 * d4343 - d4321 * d4321;
00290   if (denom > -mTolerance && denom < mTolerance)
00291     return CUBIT_FALSE;
00292   double numer = d1343 * d4321 - d1321 * d4343;
00293 
00294   t = numer / denom;
00295   if (t > -mTolerance && t < mTolerance) t = 0.0;
00296   else if ((t - 1.0) > -mTolerance &&
00297            (t - 1.0) <  mTolerance) t = 1.0;
00298   if (t < 0.0 || t > 1.0)
00299     return CUBIT_FALSE;
00300    
00301   u = (d1343 + d4321 * t) / d4343;
00302   if (u > -mTolerance && u < mTolerance) u = 0.0;
00303   else if ((u - 1.0) > -mTolerance &&
00304            (u - 1.0) <  mTolerance) u = 1.0;
00305   if (u < 0.0 || u > 1.0)
00306     return CUBIT_FALSE;
00307 
00308   return CUBIT_TRUE;
00309 }
00310 
00311 CubitStatus IntersectionTool::closest_points_on_segments( CubitVector &p0,
00312                                                           CubitVector &p1,
00313                                                           CubitVector &p2,
00314                                                           CubitVector &p3,
00315                                                           CubitVector &point_1,
00316                                                           CubitVector &point_2,
00317                                                           double &sc, double &tc)
00318 {
00319   CubitVector   u = p1 - p0;
00320   CubitVector   v = p3 - p2;
00321   CubitVector   w = p0 - p2;
00322   double    a = u%u;     //|u|  always >= 0
00323   double    b = u%v;
00324   double    c = v%v;     //|v| always >= 0
00325   double    d = u%w;
00326   double    e = v%w;
00327   double    D = a*c - b*b;       // always >= 0
00328   double    sN, sD = D;      // sc = sN / sD, default sD = D >= 0
00329   double    tN, tD = D;      // tc = tN / tD, default tD = D >= 0
00330 
00331     // compute the line parameters of the two closest points
00332   if (D < GEOMETRY_RESABS) { // the lines are almost parallel
00333     sN = 0.0;
00334     tN = e;
00335     tD = c;
00336   }
00337   else {                // get the closest points on the infinite lines
00338     sN = (b*e - c*d);
00339     tN = (a*e - b*d);
00340     if (sN < 0) {       // sc < 0 => the s=0 edge is visible
00341       sN = 0.0;
00342       tN = e;
00343       tD = c;
00344     }
00345     else if (sN > sD) {  // sc > 1 => the s=1 edge is visible
00346       sN = sD;
00347       tN = e + b;
00348       tD = c;
00349     }
00350   }
00351   
00352   if (tN < 0) {           // tc < 0 => the t=0 edge is visible
00353     tN = 0.0;
00354       // recompute sc for this edge
00355     if (-d < 0)
00356       sN = 0.0;
00357     else if (-d > a)
00358       sN = sD;
00359     else {
00360       sN = -d;
00361       sD = a;
00362     }
00363   }
00364   else if (tN > tD) {      // tc > 1 => the t=1 edge is visible
00365     tN = tD;
00366       // recompute sc for this edge
00367     if ((-d + b) < 0)
00368       sN = 0;
00369     else if ((-d + b) > a)
00370       sN = sD;
00371     else {
00372       sN = (-d + b);
00373       sD = a;
00374     }
00375   }
00376   sc = CUBIT_DBL_MAX;
00377   tc = CUBIT_DBL_MAX;
00378     //If these are going to be zero then do it...
00379   if ( sN < CUBIT_RESABS && sN > -CUBIT_RESABS )
00380     sc = 0.0;
00381   if ( tN < CUBIT_RESABS && tN > -CUBIT_RESABS )
00382     tc = 0.0;
00383 
00384     // finally do the division to get sc and tc
00385   if ( sD < CUBIT_RESABS && sD > -CUBIT_RESABS && sc != 0.0 )
00386   {
00387     PRINT_ERROR("About to divide by zero in closest_points_on_segments.\n");
00388     return CUBIT_FAILURE;
00389   }
00390   if ( tD < CUBIT_RESABS && tD > -CUBIT_RESABS && tc != 0.0 )
00391   {
00392     PRINT_ERROR("About to divide by zero in closest_points_on_segments.\n");
00393     return CUBIT_FAILURE;
00394   }
00395   if ( sc != 0.0 )
00396     sc = sN / sD;
00397   if ( tc != 0.0 )
00398     tc = tN / tD;
00399   
00400   point_1 = p0 + sc*u;
00401   point_2 = p2 + tc*v;
00402   return CUBIT_SUCCESS;
00403 }
00404 
00405 int IntersectionTool::intersect_triangle_with_ray( CubitVector &ray_origin, CubitVector &ray_direction,
00406                                                   const CubitVector *p0, const CubitVector *p1, const CubitVector *p2,
00407                                                   CubitVector* point, double &distance, int &edge_hit )
00408 {
00409     // This algorithm can be found at http://geometryalgorithms.com/
00410 
00411     CubitVector n;           // triangle vectors
00412     CubitVector w0, w;       // ray vectors
00413     double a, b;             // params to calc ray-plane intersect
00414 
00415     double tol = GEOMETRY_RESABS;
00416 
00417     // get triangle edge vectors and plane normal
00418     CubitVector u(  p1->x() - p0->x(),
00419                     p1->y() - p0->y(),
00420                     p1->z() - p0->z()); //(*p1-*p0);
00421     CubitVector v(  p2->x() - p0->x(),
00422                     p2->y() - p0->y(),
00423                     p2->z() - p0->z()); // = (*p2-*p0);
00424 
00425     n = u * v; // cross product to get normal
00426 
00427     if (n.length_squared() == 0)   // triangle is degenerate
00428         return -1;                 // do not deal with this case
00429 
00430     //dir = R.P1 - R.P0;             // ray direction vector
00431     //w0 = R.P0 - T.V0;
00432     w0 = CubitVector(ray_origin.x() - p0->x(),
00433         ray_origin.y() - p0->y(),
00434         ray_origin.z() - p0->z());
00435 
00436     a = -(n%w0);
00437     b = (n%ray_direction);
00438     if (fabs(b) < tol) {     // ray is parallel to triangle plane
00439         if (a == 0)                // ray lies in triangle plane
00440             return 2;
00441         else return 0;             // ray disjoint from plane
00442     }
00443 
00444     // get intersect point of ray with triangle plane
00445     distance = a / b;
00446     if (distance < 0.0)                   // ray goes away from triangle
00447         return 0;                  // => no intersect
00448     // for a segment, also test if (r > 1.0) => no intersect
00449 
00450     point->set(ray_origin + distance * ray_direction);           // intersect point of ray and plane
00451 
00452     // set distance to be absolute distance (if ray_direction was a unit vector)
00453     distance = distance * ray_direction.length();
00454 
00455     // is point inside facet?
00456     double uu, uv, vv, wu, wv, D;
00457     uu = u%u;
00458     uv = u%v;
00459     vv = v%v;
00460     //w = *I - T.V0;
00461     w = CubitVector(point->x() - p0->x(),
00462                     point->y() - p0->y(),
00463                     point->z() - p0->z());
00464     wu = w%u;
00465     wv = w%v;
00466     D = uv * uv - uu * vv;
00467 
00468     // get and test parametric coords
00469     double s, t;
00470     s = (uv * wv - vv * wu) / D;
00471     if (s < 0.0 || s > 1.0)        // point is outside facet
00472         return 0;
00473     t = (uv * wu - uu * wv) / D;
00474     if (t < 0.0 || (s + t) > 1.0)  // point is outside facet
00475         return 0;
00476 
00477     if (s==0)
00478         edge_hit = 2; //lies along v, edge #2
00479     if (t==0)
00480         edge_hit = 1; //lies along u, edge #1
00481     if (s+t==1)
00482         edge_hit = 3; //lies along edge #3
00483 
00484     // note:
00485     // if s and t are both 0, hit the point p0
00486     // if s=1 and t=0, hit point p1
00487     // if s=0 and t=1, hit point p2
00488 
00489     return 1; // point is in facet
00490 
00491 }
00492 
00493 int IntersectionTool::intersect_segment_with_ray( CubitVector &ray_origin, CubitVector &ray_direction,
00494                                                  const CubitVector *p0, const CubitVector *p1,
00495                                                  CubitVector* point, double &hit_distance, int &point_hit, double tol )
00496 {
00497     // This algorithm can be found at http://geometryalgorithms.com/
00498 
00499     if (tol == 0.0)
00500         tol = GEOMETRY_RESABS;
00501 
00502     CubitVector u = CubitVector(*p0, *p1);
00503     CubitVector v = ray_direction;
00504     v.normalize();
00505 
00506     CubitVector w = CubitVector(ray_origin, *p0);
00507 
00508     double sc, tc;         // sc is fraction along facet edge, tc is distance along ray
00509     
00510     double a = u%u;        // always >= 0
00511     double b = u%v;
00512     double c = v%v;        // always >= 0
00513     double d = u%w;
00514     double e = v%w;
00515     double D = a*c - b*b;  // always >= 0
00516 
00517     // compute the line parameters of the two closest points
00518     if (D < tol)
00519     {
00520         // the lines are almost parallel
00521         sc = 0.0;
00522         tc = (b>c ? d/b : e/c);   // use the largest denominator
00523     }
00524     else
00525     {
00526         sc = (b*e - c*d) / D;
00527         tc = (a*e - b*d) / D;
00528     }
00529 
00530     // get the difference of the two closest points
00531     CubitVector dP = CubitVector(w + (sc * u) - (tc * v));  // = <0 0 0> if intersection
00532 
00533     double distance = sqrt(dP % dP); // return the closest distance (0 if intersection)
00534 
00535     point->set(*p0 + (sc * u));
00536     hit_distance = tc; //distance from origin to intersection point
00537 
00538     if (distance < tol)
00539     {
00540         //check if parallel (infinite intersection)
00541         if (D < tol)
00542             return 2;
00543         //check if on edge
00544         if (sc <= 1.0 && sc >= 0.0)
00545         {
00546             if (sc==0)
00547                 point_hit = 1; //hit point p0
00548             if (sc==1)
00549                 point_hit = 2; //hit point p1
00550 
00551             return 1;
00552         }
00553         else
00554             return 0;
00555     }
00556 
00557     return 0;
00558 }
00559 
00560 int IntersectionTool::intersect_point_with_ray( CubitVector &ray_origin, CubitVector &ray_direction, 
00561       const CubitVector* point, double &distance, double tol)
00562 {
00563     if (tol == 0.0)
00564         tol = GEOMETRY_RESABS;
00565 
00566     //Does the ray pass through the Point?
00567     // Calc distance from ray origin to Point.
00568     // Then compute coord's of point along ray that distance.
00569     // Calc distance between Point and this ray-point. If less than tolerance, a hit.
00570 
00571     CubitVector pointB;
00572     double dist1 = point->distance_between(ray_origin);
00573     ray_origin.next_point(ray_direction, dist1, pointB);
00574 
00575     if ( pointB.distance_between_squared(*point) <= (tol*tol) )
00576     {
00577         distance = dist1;
00578         return 1;
00579     }
00580     
00581     return 0;
00582 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines