moab
GeomUtil.cpp
Go to the documentation of this file.
00001 /*
00002  * MOAB, a Mesh-Oriented datABase, is a software component for creating,
00003  * storing and accessing finite element mesh data.
00004  * 
00005  * Copyright 2004 Sandia Corporation.  Under the terms of Contract
00006  * DE-AC04-94AL85000 with Sandia Coroporation, the U.S. Government
00007  * retains certain rights in this software.
00008  * 
00009  * This library is free software; you can redistribute it and/or
00010  * modify it under the terms of the GNU Lesser General Public
00011  * License as published by the Free Software Foundation; either
00012  * version 2.1 of the License, or (at your option) any later version.
00013  * 
00014  */
00015 
00021 #include "moab/CartVect.hpp"
00022 #include "moab/CN.hpp"
00023 #include "moab/GeomUtil.hpp"
00024 #include "moab/Matrix3.hpp"
00025 #include <cmath>
00026 #include <algorithm>
00027 #include <assert.h>
00028 #include <iostream>
00029 #include <limits>
00030 
00031 #if defined(_MSC_VER) || defined(__MINGW32__)
00032 #  include <float.h>
00033 #  define finite(A) _finite(A)
00034 #endif
00035 
00036 namespace moab {
00037 
00038 namespace GeomUtil {
00039 
00040 static inline 
00041 void min_max_3( double a, double b, double c, double& min, double& max )
00042 {
00043   if (a < b) {
00044     if (a < c) {
00045       min = a;
00046       max = b > c ? b : c;
00047     }
00048     else {
00049       min = c;
00050       max = b;
00051     }
00052   }
00053   else if (b < c) {
00054     min = b;
00055     max = a > c ? a : c;
00056   }
00057   else {
00058     min = c;
00059     max = a;
00060   }
00061 }
00062 
00063 static inline
00064 double dot_abs( const CartVect& u, const CartVect& v )
00065   { return fabs(u[0]*v[0]) + fabs(u[1]*v[1]) + fabs(u[2]*v[2]); }
00066 
00067 bool segment_box_intersect( CartVect box_min,
00068                             CartVect box_max,
00069                             const CartVect& seg_pt,
00070                             const CartVect& seg_unit_dir,
00071                             double& seg_start, double& seg_end )
00072 {
00073     // translate so that seg_pt is at origin
00074   box_min -= seg_pt;
00075   box_max -= seg_pt;
00076   
00077   for (unsigned i = 0; i < 3; ++i) {  // X, Y, and Z slabs
00078 
00079       // intersect line with slab planes
00080     const double t_min = box_min[i] / seg_unit_dir[i];
00081     const double t_max = box_max[i] / seg_unit_dir[i];
00082     
00083       // check if line is parallel to planes
00084     if (!finite(t_min)) {
00085       if (box_min[i] > 0.0 || box_max[i] < 0.0)
00086         return false; 
00087       continue;
00088     }
00089 
00090     if (seg_unit_dir[i] < 0) {
00091       if (t_min < seg_end) 
00092         seg_end = t_min;
00093       if (t_max > seg_start)
00094         seg_start = t_max;
00095     }
00096     else { // seg_unit_dir[i] > 0
00097       if (t_min > seg_start)
00098         seg_start = t_min; 
00099       if (t_max < seg_end)
00100         seg_end = t_max;
00101     }
00102   }
00103 
00104   return seg_start <= seg_end;
00105 }
00106 
00107 /* Function to return the vertex with the lowest coordinates. To force the same
00108    ray-edge computation, the Plücker test needs to use consistent edge 
00109    representation. This would be more simple with MOAB handles instead of 
00110    coordinates... */
00111 inline bool first( const CartVect& a, const CartVect& b) {
00112   if(a[0] < b[0]) {
00113     return true;
00114   } else if(a[0] == b[0]) {
00115     if(a[1] < b[1]) {
00116       return true;
00117     } else if(a[1] == b[1]) {
00118       if(a[2] < b[2]) {
00119     return true;
00120       } else {
00121         return false;
00122       }
00123     } else {
00124       return false;
00125     }
00126   } else {
00127     return false;
00128   }
00129 }
00130 
00131 /* This test uses the same edge-ray computation for adjacent triangles so that
00132    rays passing close to edges/nodes are handled consistently.
00133 
00134    Reports intersection type for post processing of special cases. Optionally 
00135    screen by orientation and negative/nonnegative distance limits.
00136 
00137    If screening by orientation, substantial pruning can occur. Indicate
00138    desired orientation by passing 1 (forward), -1 (reverse), or 0 (no preference).
00139    Note that triangle orientation is not always the same as surface
00140    orientation due to non-manifold surfaces.
00141 
00142    N. Platis and T. Theoharis, "Fast Ray-Tetrahedron Intersection using Plücker
00143    Coordinates", Journal of Graphics Tools, Vol. 8, Part 4, Pages 37-48 (2003). */
00144 bool plucker_ray_tri_intersect( const CartVect vertices[3],
00145                                 const CartVect& origin,
00146                                 const CartVect& direction,
00147                                 double /* tolerance */,
00148                                 double& dist_out,
00149                                 const double* nonneg_ray_len,
00150                                 const double* neg_ray_len,
00151                                 const int*    orientation,
00152                     intersection_type* type ) {
00153 
00154   const CartVect raya = direction;
00155   const CartVect rayb = direction*origin;
00156 
00157   // edge 0
00158   double pip0;
00159   if(first(vertices[0],vertices[1])) {
00160     const CartVect edge0a = vertices[1]-vertices[0];
00161     const CartVect edge0b = edge0a*vertices[0];
00162     pip0 = raya % edge0b + rayb % edge0a;
00163   } else {
00164     const CartVect edge0a = vertices[0]-vertices[1];
00165     const CartVect edge0b = edge0a*vertices[1];
00166     pip0 = raya % edge0b + rayb % edge0a;
00167     pip0 = -pip0;
00168   }
00169 
00170   // try to exit early
00171   if(orientation && (*orientation)*pip0 > 0) {
00172     if(type) *type = NONE;
00173     return false;
00174   }
00175 
00176   // edge 1
00177   double pip1;
00178   if(first(vertices[1],vertices[2])) {
00179     const CartVect edge1a = vertices[2]-vertices[1];
00180     const CartVect edge1b = edge1a*vertices[1];
00181     pip1 = raya % edge1b + rayb % edge1a;
00182   } else {
00183     const CartVect edge1a = vertices[1]-vertices[2];
00184     const CartVect edge1b = edge1a*vertices[2];
00185     pip1 = raya % edge1b + rayb % edge1a;
00186     pip1 = -pip1;
00187   }
00188 
00189   // try to exit early
00190   if(orientation) {
00191     if( (*orientation)*pip1 > 0) {
00192       if(type) *type = NONE;
00193       return false;
00194     }
00195   // If the orientation is not specified, all pips must be the same sign or zero.
00196   } else if( (0.0<pip0 && 0.0>pip1) || (0.0>pip0 && 0.0<pip1) ) {
00197     if(type) *type = NONE;
00198     return false;
00199   }
00200 
00201   // edge 2
00202   double pip2;
00203   if(first(vertices[2],vertices[0])) {
00204     const CartVect edge2a = vertices[0]-vertices[2];
00205     const CartVect edge2b = edge2a*vertices[2];
00206     pip2 = raya % edge2b + rayb % edge2a;
00207   } else {
00208     const CartVect edge2a = vertices[2]-vertices[0];
00209     const CartVect edge2b = edge2a*vertices[0];
00210     pip2 = raya % edge2b + rayb % edge2a;
00211     pip2 = -pip2;
00212   }
00213 
00214   // try to exit early
00215   if(orientation) {
00216     if( (*orientation)*pip2 > 0) {
00217       if(type) *type = NONE;
00218       return false;
00219     }
00220   // If the orientation is not specified, all pips must be the same sign or zero.
00221   } else if( (0.0<pip1 && 0.0>pip2) || (0.0>pip1 && 0.0<pip2) ||
00222              (0.0<pip0 && 0.0>pip2) || (0.0>pip0 && 0.0<pip2) ) {
00223     if(type) *type = NONE;
00224     return false;
00225   }
00226 
00227   // check for coplanar case to avoid dividing by zero
00228   if(0==pip0 && 0==pip1 && 0==pip2) {
00229     //std::cout << "plucker: coplanar" << std::endl;
00230     if(type) *type = NONE;
00231     return false;
00232   }
00233 
00234   // get the distance to intersection
00235   const double inverse_sum = 1.0/(pip0+pip1+pip2);
00236   assert(0.0 != inverse_sum);
00237   const CartVect intersection(pip0*inverse_sum*vertices[2]+ 
00238                           pip1*inverse_sum*vertices[0]+
00239                   pip2*inverse_sum*vertices[1]);
00240 
00241   // To minimize numerical error, get index of largest magnitude direction.
00242   int idx = 0;
00243   double max_abs_dir = 0;
00244   for(unsigned int i=0; i<3; ++i) {
00245     if( fabs(direction[i]) > max_abs_dir ) {
00246       idx = i;
00247       max_abs_dir = fabs(direction[i]);
00248     }
00249   } 
00250   const double dist = (intersection[idx]-origin[idx])/direction[idx];
00251 
00252   // is the intersection within distance limits?
00253   if(nonneg_ray_len && *nonneg_ray_len<dist) {
00254     if(type) *type = NONE;
00255     return false;
00256   }
00257   if(neg_ray_len && *neg_ray_len>=dist) {
00258     if(type) *type = NONE;
00259     return false;
00260 
00261   // Unless a neg_ray_len is used, don't return negative distances
00262   } else if (!neg_ray_len && 0>dist) {
00263     if(type) *type = NONE;
00264     return false;
00265   }    
00266   dist_out = dist;
00267  
00268   // check for special cases
00269   if(0==pip0 || 0==pip1 || 0==pip2) {
00270     if       (0==pip0 && 0==pip1) {
00271       //std::cout << "plucker: node1" << std::endl;
00272       if(type) *type = NODE1;
00273       return true;
00274     } else if(0==pip1 && 0==pip2) {
00275       //std::cout << "plucker: node2" << std::endl;
00276       if(type) *type = NODE2;
00277       return true;
00278     } else if(0==pip2 && 0==pip0) {
00279       //std::cout << "plucker: node0" << std::endl;
00280       if(type) *type = NODE0;
00281       return true;
00282     } else if(0==pip0) {
00283       //std::cout << "plucker: edge0" << std::endl;
00284       if(type) *type = EDGE0;
00285       return true;
00286     } else if(0==pip1) {
00287       //std::cout << "plucker: edge1" << std::endl;
00288       if(type) *type = EDGE1;
00289       return true;
00290     } else if(0==pip2) {
00291       //std::cout << "plucker: edge2" << std::endl;
00292       if(type) *type = EDGE2;
00293       return true;
00294     }
00295   }
00296 
00297   // if here, ray intersects interior of tri
00298   if(type) *type = INTERIOR;
00299   return true;
00300 }
00301 
00302 /* Impelementation copied from cgmMC ray_tri_contact (overlap.C) */
00303 bool ray_tri_intersect( const CartVect vertices[3],
00304                         const CartVect& b,
00305                         const CartVect& v,
00306                         double /*tolerance*/,
00307                         double& t_out,
00308                         const double* ray_length)
00309 {
00310   const CartVect p0 = vertices[0] - vertices[1]; // abc
00311   const CartVect p1 = vertices[0] - vertices[2]; // def
00312                                                    // ghi<-v
00313   const CartVect p = vertices[0] - b;            // jkl
00314   const CartVect c = p1 * v;                     // eiMinushf,gfMinusdi,dhMinuseg
00315   const double mP = p0 % c;
00316   const double betaP = p % c;
00317   if (mP > 0) {
00318     if (betaP < 0)
00319       return false;
00320   }
00321   else if (mP < 0) {
00322     if (betaP > 0)
00323       return false;
00324   }
00325   else {
00326     return false;
00327   }
00328   
00329   const CartVect d = p0 * p; // jcMinusal,blMinuskc,akMinusjb
00330   double gammaP = v % d;
00331   if (mP > 0) {
00332     if (gammaP < 0 || betaP + gammaP > mP)
00333       return false;
00334   }
00335   else if (betaP + gammaP < mP || gammaP > 0)
00336     return false;
00337   
00338   const double tP = p1 % d;
00339   const double m = 1.0 / mP;
00340   const double beta = betaP * m;
00341   const double gamma = gammaP * m;
00342   const double t = -tP * m;
00343   if (ray_length && t > *ray_length)
00344     return false;
00345   
00346   if (beta < 0 || gamma < 0 ||
00347       beta + gamma > 1 ||
00348       t < 0.0)
00349     return false;
00350   
00351   t_out = t;
00352   return true;
00353 }
00354 
00355 bool ray_box_intersect( const CartVect& box_min,
00356                         const CartVect& box_max,
00357                         const CartVect& ray_pt,
00358                         const CartVect& ray_dir,
00359                         double& t_enter, double& t_exit )
00360 {
00361   const double epsilon = 1e-12;
00362   double t1, t2;
00363 
00364   // Use 'slabs' method from 13.6.1 of Akenine-Moller
00365   t_enter = 0.0;
00366   t_exit  = std::numeric_limits<double>::infinity();
00367   
00368   // Intersect with each pair of axis-aligned planes bounding
00369   // opposite faces of the leaf box
00370   bool ray_is_valid = false; // is ray direction vector zero?
00371   for (int axis = 0; axis < 3; ++axis) {
00372     if (fabs(ray_dir[axis]) < epsilon) { // ray parallel to planes
00373       if (ray_pt[axis] >= box_min[axis] &&
00374           ray_pt[axis] <= box_max[axis])
00375         continue;
00376       else
00377         return false;
00378     }
00379       
00380       // find t values at which ray intersects each plane
00381     ray_is_valid = true;
00382     t1 = (box_min[axis] - ray_pt[axis]) / ray_dir[axis];
00383     t2 = (box_max[axis] - ray_pt[axis]) / ray_dir[axis];
00384     
00385       // t_enter = max( t_enter_x, t_enter_y, t_enter_z )
00386       // t_exit  = min( t_exit_x, t_exit_y, t_exit_z )
00387       //   where
00388       // t_enter_x = min( t1_x, t2_x );
00389       // t_exit_x  = max( t1_x, t2_x )
00390     if (t1 < t2) {
00391       if (t_enter < t1)
00392         t_enter = t1;
00393       if (t_exit > t2)
00394         t_exit = t2;
00395     }
00396     else {
00397       if (t_enter < t2)
00398         t_enter = t2;
00399       if (t_exit > t1)
00400         t_exit = t1;
00401     }
00402   }
00403   
00404   return ray_is_valid && (t_enter <= t_exit);
00405 }
00406 
00407 
00408 bool box_plane_overlap( const CartVect& normal,
00409                         double d,
00410                         CartVect min,
00411                         CartVect max )
00412 {
00413   if (normal[0] < 0.0)
00414     std::swap( min[0], max[0] );
00415   if (normal[1] < 0.0)
00416     std::swap( min[1], max[1] );
00417   if (normal[2] < 0.0)
00418     std::swap( min[2], max[2] );
00419   
00420   return (normal % min <= -d) && (normal % max >= -d);
00421 }
00422 
00423 
00424 #define CHECK_RANGE( A, B, R ) \
00425   if ((A) < (B)) { \
00426     if ((A) > (R) || (B) < -(R)) \
00427       return false; \
00428   } \
00429   else if ((B) > (R) || (A) < -(R)) \
00430     return false
00431 
00432 /* Adapted from: http://jgt.akpeters.com/papers/AkenineMoller01/tribox.html
00433  * Use separating axis theorem to test for overlap between triangle
00434  * and axis-aligned box.
00435  *
00436  * Test for overlap in these directions:
00437  * 1) {x,y,z}-directions 
00438  * 2) normal of triangle
00439  * 3) crossprod of triangle edge with {x,y,z}-direction
00440  */
00441 bool box_tri_overlap( const CartVect vertices[3],
00442                       const CartVect& box_center,
00443                       const CartVect& box_dims )
00444 {
00445     // translate everthing such that box is centered at origin
00446   const CartVect v0( vertices[0] - box_center );
00447   const CartVect v1( vertices[1] - box_center );
00448   const CartVect v2( vertices[2] - box_center );
00449 
00450   // do case 1) tests
00451   if (v0[0] > box_dims[0] && v1[0] > box_dims[0] && v2[0] > box_dims[0])
00452     return false;
00453   if (v0[1] > box_dims[1] && v1[1] > box_dims[1] && v2[1] > box_dims[1])
00454     return false;
00455   if (v0[2] > box_dims[2] && v1[2] > box_dims[2] && v2[2] > box_dims[2])
00456     return false;
00457   if (v0[0] < -box_dims[0] && v1[0] < -box_dims[0] && v2[0] < -box_dims[0])
00458     return false;
00459   if (v0[1] < -box_dims[1] && v1[1] < -box_dims[1] && v2[1] < -box_dims[1])
00460     return false;
00461   if (v0[2] < -box_dims[2] && v1[2] < -box_dims[2] && v2[2] < -box_dims[2])
00462     return false;
00463   
00464     // compute triangle edge vectors
00465   const CartVect e0( vertices[1] - vertices[0] );
00466   const CartVect e1( vertices[2] - vertices[1] );
00467   const CartVect e2( vertices[0] - vertices[2] );
00468   
00469     // do case 3) tests 
00470   double fex, fey, fez, p0, p1, p2, rad;
00471   fex = fabs(e0[0]);
00472   fey = fabs(e0[1]);
00473   fez = fabs(e0[2]);
00474   
00475   p0 = e0[2]*v0[1] - e0[1]*v0[2];
00476   p2 = e0[2]*v2[1] - e0[1]*v2[2];
00477   rad = fez * box_dims[1] + fey * box_dims[2];
00478   CHECK_RANGE( p0, p2, rad );
00479   
00480   p0 = -e0[2]*v0[0] + e0[0]*v0[2];
00481   p2 = -e0[2]*v2[0] + e0[0]*v2[2];
00482   rad = fez * box_dims[0] + fex * box_dims[2];
00483   CHECK_RANGE( p0, p2, rad );
00484     
00485   p1 = e0[1]*v1[0] - e0[0]*v1[1];
00486   p2 = e0[1]*v2[0] - e0[0]*v2[1];
00487   rad = fey * box_dims[0] + fex * box_dims[1];
00488   CHECK_RANGE( p1, p2, rad );
00489   
00490   fex = fabs(e1[0]);
00491   fey = fabs(e1[1]);
00492   fez = fabs(e1[2]);
00493   
00494   p0 = e1[2]*v0[1] - e1[1]*v0[2];
00495   p2 = e1[2]*v2[1] - e1[1]*v2[2];
00496   rad = fez * box_dims[1] + fey * box_dims[2];
00497   CHECK_RANGE( p0, p2, rad );
00498   
00499   p0 = -e1[2]*v0[0] + e1[0]*v0[2];
00500   p2 = -e1[2]*v2[0] + e1[0]*v2[2];
00501   rad = fez * box_dims[0] + fex * box_dims[2];
00502   CHECK_RANGE( p0, p2, rad );
00503   
00504   p0 = e1[1]*v0[0] - e1[0]*v0[1];
00505   p1 = e1[1]*v1[0] - e1[0]*v1[1];
00506   rad = fey * box_dims[0] + fex * box_dims[1];
00507   CHECK_RANGE( p0, p1, rad );
00508   
00509   fex = fabs(e2[0]);
00510   fey = fabs(e2[1]);
00511   fez = fabs(e2[2]);
00512   
00513   p0 = e2[2]*v0[1] - e2[1]*v0[2];
00514   p1 = e2[2]*v1[1] - e2[1]*v1[2];
00515   rad = fez * box_dims[1] + fey * box_dims[2];
00516   CHECK_RANGE( p0, p1, rad );
00517   
00518   p0 = -e2[2]*v0[0] + e2[0]*v0[2];
00519   p1 = -e2[2]*v1[0] + e2[0]*v1[2];
00520   rad = fez * box_dims[0] + fex * box_dims[2];
00521   CHECK_RANGE( p0, p1, rad );
00522   
00523   p1 = e2[1]*v1[0] - e2[0]*v1[1];
00524   p2 = e2[1]*v2[0] - e2[0]*v2[1];
00525   rad = fey * box_dims[0] + fex * box_dims[1];
00526   CHECK_RANGE( p1, p2, rad );
00527   
00528   // do case 2) test
00529   CartVect n = e0 * e1;
00530   return box_plane_overlap( n, -(n % v0), -box_dims, box_dims );
00531 }
00532   
00533 
00534 bool box_tri_overlap( const CartVect  triangle_corners[3],
00535                       const CartVect& box_min_corner,
00536                       const CartVect& box_max_corner,
00537                       double            tolerance )
00538 {
00539   const CartVect box_center = 0.5 * (box_max_corner + box_min_corner);
00540   const CartVect box_hf_dim = 0.5 * (box_max_corner - box_min_corner);
00541   return box_tri_overlap( triangle_corners,
00542                           box_center,
00543                           box_hf_dim + CartVect(tolerance) );
00544 } 
00545 
00546 bool box_elem_overlap( const CartVect *elem_corners,
00547                        EntityType elem_type,
00548                        const CartVect& center,
00549                        const CartVect& dims )
00550 {
00551 
00552   switch (elem_type) {
00553     case MBTRI:
00554       return box_tri_overlap( elem_corners, center, dims );
00555     case MBTET:
00556       return box_tet_overlap( elem_corners, center, dims );
00557     case MBHEX:
00558       return box_hex_overlap( elem_corners, center, dims );
00559     case MBPOLYGON:
00560     case MBPOLYHEDRON:
00561       assert(false);
00562       return false;
00563     default:
00564       return box_linear_elem_overlap( elem_corners, elem_type, center, dims );
00565   }
00566 }
00567 
00568 static inline CartVect quad_norm( const CartVect& v1,
00569                                     const CartVect& v2,
00570                                     const CartVect& v3,
00571                                     const CartVect& v4 )
00572 { return (-v1+v2+v3-v4) * (-v1-v2+v3+v4); }
00573 
00574 static inline CartVect tri_norm( const CartVect& v1,
00575                                    const CartVect& v2,
00576                                    const CartVect& v3 )
00577 { return (v2-v1) * (v3-v1); }
00578 
00579 
00580 bool box_linear_elem_overlap( const CartVect *elem_corners,
00581                               EntityType type,
00582                               const CartVect& box_center,
00583                               const CartVect& box_halfdims )
00584 {
00585   CartVect corners[8];
00586   const unsigned num_corner = CN::VerticesPerEntity( type );
00587   assert( num_corner <= sizeof(corners)/sizeof(corners[0]) );
00588   for (unsigned i = 0; i < num_corner; ++i)
00589     corners[i] = elem_corners[i] - box_center;
00590   return box_linear_elem_overlap( corners, type, box_halfdims );
00591 }
00592         
00593 
00594 bool box_linear_elem_overlap( const CartVect *elem_corners,
00595                               EntityType type,
00596                               const CartVect& dims )
00597 {
00598     // Do Separating Axis Theorem:
00599     // If the element and the box overlap, then the 1D projections
00600     // onto at least one of the axes in the following three sets
00601     // must overlap (assuming convex polyhedral element).
00602     // 1) The normals of the faces of the box (the principal axes)
00603     // 2) The crossproduct of each element edge with each box edge
00604     //    (crossproduct of each edge with each principal axis)
00605     // 3) The normals of the faces of the element
00606 
00607   unsigned e, f;             // loop counters
00608   int i;
00609   double dot, cross[2], tmp;
00610   CartVect norm;
00611   int indices[4]; // element edge/face vertex indices
00612   
00613     // test box face normals (principal axes)
00614   const int num_corner = CN::VerticesPerEntity( type );
00615   int not_less[3] = { num_corner, num_corner, num_corner }; 
00616   int not_greater[3] = { num_corner, num_corner, num_corner };
00617   int not_inside;
00618   for (i = 0; i < num_corner; ++i) { // for each element corner
00619     not_inside = 3;
00620     
00621     if (elem_corners[i][0] < -dims[0])
00622       --not_less[0];
00623     else if (elem_corners[i][0] > dims[0])
00624       --not_greater[0];
00625     else
00626       --not_inside;
00627       
00628     if (elem_corners[i][1] < -dims[1])
00629       --not_less[1];
00630     else if (elem_corners[i][1] > dims[1])
00631       --not_greater[1];
00632     else
00633       --not_inside;
00634       
00635     if (elem_corners[i][2] < -dims[2])
00636       --not_less[2];
00637     else if (elem_corners[i][2] > dims[2])
00638       --not_greater[2];
00639     else
00640       --not_inside;
00641     
00642     if (!not_inside)
00643       return true;
00644   }
00645     // If all points less than min_x of box, then
00646     // not_less[0] == 0, and therfore
00647     // the following product is zero.
00648   if (not_greater[0] * not_greater[1] * not_greater[2] * 
00649          not_less[0] *    not_less[1] *    not_less[2] == 0)
00650     return false;
00651  
00652     // Test edge-edge crossproducts
00653     
00654     // Edge directions for box are principal axis, so 
00655     // for each element edge, check along the cross-product
00656     // of that edge with each of the tree principal axes.
00657   const unsigned num_edge = CN::NumSubEntities( type, 1 );
00658   for (e = 0; e < num_edge; ++e) { // for each element edge
00659       // get which element vertices bound the edge
00660     CN::SubEntityVertexIndices( type, 1, e, indices );
00661 
00662       // X-Axis
00663 
00664       // calculate crossproduct: axis x (v1 - v0),
00665       // where v1 and v0 are edge vertices.
00666     cross[0] = elem_corners[indices[0]][2] - elem_corners[indices[1]][2];
00667     cross[1] = elem_corners[indices[1]][1] - elem_corners[indices[0]][1];
00668       // skip if parallel
00669     if ((cross[0]*cross[0] + cross[1]*cross[1]) >= std::numeric_limits<double>::epsilon()) {
00670       dot = fabs(cross[0] * dims[1]) + fabs(cross[1] * dims[2]);
00671       not_less[0] = not_greater[0] = num_corner - 1;
00672       for (i = (indices[0]+1)%num_corner; i != indices[0]; i = (i+1)%num_corner) { // for each element corner
00673         tmp = cross[0] * elem_corners[i][1] + cross[1] * elem_corners[i][2];
00674         not_less[0] -= (tmp < -dot);
00675         not_greater[0] -= (tmp > dot);
00676       }
00677 
00678       if (not_less[0] * not_greater[0] == 0)
00679         return false;
00680     }
00681 
00682       // Y-Axis
00683 
00684       // calculate crossproduct: axis x (v1 - v0),
00685       // where v1 and v0 are edge vertices.
00686     cross[0] = elem_corners[indices[0]][0] - elem_corners[indices[1]][0];
00687     cross[1] = elem_corners[indices[1]][2] - elem_corners[indices[0]][2];
00688       // skip if parallel
00689     if ((cross[0]*cross[0] + cross[1]*cross[1]) >= std::numeric_limits<double>::epsilon()) {
00690       dot = fabs(cross[0] * dims[2]) + fabs(cross[1] * dims[0]);
00691       not_less[0] = not_greater[0] = num_corner - 1;
00692       for (i = (indices[0]+1)%num_corner; i != indices[0]; i = (i+1)%num_corner) { // for each element corner
00693         tmp = cross[0] * elem_corners[i][2] + cross[1] * elem_corners[i][0];
00694         not_less[0] -= (tmp < -dot);
00695         not_greater[0] -= (tmp > dot);
00696       }
00697 
00698       if (not_less[0] * not_greater[0] == 0)
00699         return false;
00700     }
00701 
00702       // Z-Axis
00703 
00704       // calculate crossproduct: axis x (v1 - v0),
00705       // where v1 and v0 are edge vertices.
00706     cross[0] = elem_corners[indices[0]][1] - elem_corners[indices[1]][1];
00707     cross[1] = elem_corners[indices[1]][0] - elem_corners[indices[0]][0];
00708       // skip if parallel
00709     if ((cross[0]*cross[0] + cross[1]*cross[1]) >= std::numeric_limits<double>::epsilon()) {
00710       dot = fabs(cross[0] * dims[0]) + fabs(cross[1] * dims[1]);
00711       not_less[0] = not_greater[0] = num_corner - 1;
00712       for (i = (indices[0]+1)%num_corner; i != indices[0]; i = (i+1)%num_corner) { // for each element corner
00713         tmp = cross[0] * elem_corners[i][0] + cross[1] * elem_corners[i][1];
00714         not_less[0] -= (tmp < -dot);
00715         not_greater[0] -= (tmp > dot);
00716       }
00717 
00718       if (not_less[0] * not_greater[0] == 0)
00719         return false;
00720     }
00721   }
00722   
00723   
00724     // test element face normals
00725   const unsigned num_face = CN::NumSubEntities( type, 2 );
00726   for (f = 0; f < num_face; ++f) {
00727     CN::SubEntityVertexIndices( type, 2, f, indices );
00728     switch (CN::SubEntityType( type, 2, f )) {
00729       case MBTRI:
00730         norm = tri_norm( elem_corners[indices[0]], 
00731                          elem_corners[indices[1]], 
00732                          elem_corners[indices[2]] );
00733         break;
00734       case MBQUAD:
00735         norm = quad_norm( elem_corners[indices[0]], 
00736                           elem_corners[indices[1]], 
00737                           elem_corners[indices[2]], 
00738                           elem_corners[indices[3]] );
00739         break;
00740       default:
00741         assert(false);
00742         continue;
00743     }
00744     
00745     dot = dot_abs(norm, dims);
00746     
00747     // for each element vertex
00748     not_less[0] = not_greater[0] = num_corner;
00749     for (i = 0; i < num_corner; ++i) { 
00750       tmp = norm % elem_corners[i];
00751       not_less[0] -= (tmp < -dot);
00752       not_greater[0] -= (tmp > dot);
00753     }
00754 
00755     if (not_less[0] * not_greater[0] == 0)
00756       return false;
00757   }
00758 
00759     // Overlap on all tested axes.
00760   return true;
00761 }
00762  
00763 
00764 bool box_hex_overlap( const CartVect *elem_corners,
00765                       const CartVect& center,
00766                       const CartVect& dims )
00767 {
00768     // Do Separating Axis Theorem:
00769     // If the element and the box overlap, then the 1D projections
00770     // onto at least one of the axes in the following three sets
00771     // must overlap (assuming convex polyhedral element).
00772     // 1) The normals of the faces of the box (the principal axes)
00773     // 2) The crossproduct of each element edge with each box edge
00774     //    (crossproduct of each edge with each principal axis)
00775     // 3) The normals of the faces of the element
00776 
00777   unsigned i, e, f;             // loop counters
00778   double dot, cross[2], tmp;
00779   CartVect norm;
00780   const CartVect corners[8] = { elem_corners[0] - center,
00781                                   elem_corners[1] - center,
00782                                   elem_corners[2] - center,
00783                                   elem_corners[3] - center,
00784                                   elem_corners[4] - center,
00785                                   elem_corners[5] - center,
00786                                   elem_corners[6] - center,
00787                                   elem_corners[7] - center };
00788   
00789     // test box face normals (principal axes)
00790   int not_less[3] = { 8, 8, 8 }; 
00791   int not_greater[3] = { 8, 8, 8 };
00792   int not_inside;
00793   for (i = 0; i < 8; ++i) { // for each element corner
00794     not_inside = 3;
00795     
00796     if (corners[i][0] < -dims[0])
00797       --not_less[0];
00798     else if (corners[i][0] > dims[0])
00799       --not_greater[0];
00800     else
00801       --not_inside;
00802       
00803     if (corners[i][1] < -dims[1])
00804       --not_less[1];
00805     else if (corners[i][1] > dims[1])
00806       --not_greater[1];
00807     else
00808       --not_inside;
00809       
00810     if (corners[i][2] < -dims[2])
00811       --not_less[2];
00812     else if (corners[i][2] > dims[2])
00813       --not_greater[2];
00814     else
00815       --not_inside;
00816     
00817     if (!not_inside)
00818       return true;
00819   }
00820     // If all points less than min_x of box, then
00821     // not_less[0] == 0, and therfore
00822     // the following product is zero.
00823   if (not_greater[0] * not_greater[1] * not_greater[2] * 
00824          not_less[0] *    not_less[1] *    not_less[2] == 0)
00825     return false;
00826  
00827     // Test edge-edge crossproducts
00828   const unsigned edges[12][2] = { { 0, 1 }, { 0, 4 }, { 0, 3 },
00829                                   { 2, 3 }, { 2, 1 }, { 2, 6 },
00830                                   { 5, 6 }, { 5, 1 }, { 5, 4 },
00831                                   { 7, 4 }, { 7, 3 }, { 7, 6 } };
00832                              
00833     // Edge directions for box are principal axis, so 
00834     // for each element edge, check along the cross-product
00835     // of that edge with each of the tree principal axes.
00836   for (e = 0; e < 12; ++e) { // for each element edge
00837       // get which element vertices bound the edge
00838     const CartVect& v0 = corners[ edges[e][0] ];
00839     const CartVect& v1 = corners[ edges[e][1] ];
00840 
00841       // X-Axis
00842 
00843       // calculate crossproduct: axis x (v1 - v0),
00844       // where v1 and v0 are edge vertices.
00845     cross[0] = v0[2] - v1[2];
00846     cross[1] = v1[1] - v0[1];
00847       // skip if parallel
00848     if ((cross[0]*cross[0] + cross[1]*cross[1]) >= std::numeric_limits<double>::epsilon()) {
00849       dot = fabs(cross[0] * dims[1]) + fabs(cross[1] * dims[2]);
00850       not_less[0] = not_greater[0] = 7;
00851       for (i = (edges[e][0]+1)%8; i != edges[e][0]; i = (i+1)%8) { // for each element corner
00852         tmp = cross[0] * corners[i][1] + cross[1] * corners[i][2];
00853         not_less[0] -= (tmp < -dot);
00854         not_greater[0] -= (tmp > dot);
00855       }
00856 
00857       if (not_less[0] * not_greater[0] == 0)
00858         return false;
00859     }
00860 
00861       // Y-Axis
00862 
00863       // calculate crossproduct: axis x (v1 - v0),
00864       // where v1 and v0 are edge vertices.
00865     cross[0] = v0[0] - v1[0];
00866     cross[1] = v1[2] - v0[2];
00867       // skip if parallel
00868     if ((cross[0]*cross[0] + cross[1]*cross[1]) >= std::numeric_limits<double>::epsilon()) {
00869       dot = fabs(cross[0] * dims[2]) + fabs(cross[1] * dims[0]);
00870       not_less[0] = not_greater[0] = 7;
00871       for (i = (edges[e][0]+1)%8; i != edges[e][0]; i = (i+1)%8) { // for each element corner
00872         tmp = cross[0] * corners[i][2] + cross[1] * corners[i][0];
00873         not_less[0] -= (tmp < -dot);
00874         not_greater[0] -= (tmp > dot);
00875       }
00876 
00877       if (not_less[0] * not_greater[0] == 0)
00878         return false;
00879     }
00880 
00881       // Z-Axis
00882 
00883       // calculate crossproduct: axis x (v1 - v0),
00884       // where v1 and v0 are edge vertices.
00885     cross[0] = v0[1] - v1[1];
00886     cross[1] = v1[0] - v0[0];
00887       // skip if parallel
00888     if ((cross[0]*cross[0] + cross[1]*cross[1]) >= std::numeric_limits<double>::epsilon()) {
00889       dot = fabs(cross[0] * dims[0]) + fabs(cross[1] * dims[1]);
00890       not_less[0] = not_greater[0] = 7;
00891       for (i = (edges[e][0]+1)%8; i != edges[e][0]; i = (i+1)%8) { // for each element corner
00892         tmp = cross[0] * corners[i][0] + cross[1] * corners[i][1];
00893         not_less[0] -= (tmp < -dot);
00894         not_greater[0] -= (tmp > dot);
00895       }
00896 
00897       if (not_less[0] * not_greater[0] == 0)
00898         return false;
00899     }
00900   }
00901   
00902   
00903     // test element face normals
00904   const unsigned faces[6][4] = { { 0, 1, 2, 3 },
00905                                  { 0, 1, 5, 4 },
00906                                  { 1, 2, 6, 5 },
00907                                  { 2, 6, 7, 3 },
00908                                  { 3, 7, 4, 0 },
00909                                  { 7, 4, 5, 6 } };
00910   for (f = 0; f < 6; ++f) {
00911     norm = quad_norm( corners[faces[f][0]], 
00912                       corners[faces[f][1]], 
00913                       corners[faces[f][2]], 
00914                       corners[faces[f][3]] );
00915     
00916     dot = dot_abs(norm, dims);
00917    
00918     // for each element vertex
00919     not_less[0] = not_greater[0] = 8;
00920     for (i = 0; i < 8; ++i) { 
00921       tmp = norm % corners[i];
00922       not_less[0] -= (tmp < -dot);
00923       not_greater[0] -= (tmp > dot);
00924     }
00925 
00926     if (not_less[0] * not_greater[0] == 0)
00927       return false;
00928   }
00929 
00930     // Overlap on all tested axes.
00931   return true;
00932 }
00933 
00934 static inline 
00935 bool box_tet_overlap_edge( const CartVect& dims,
00936                            const CartVect& edge,
00937                            const CartVect& ve,
00938                            const CartVect& v1,
00939                            const CartVect& v2 )
00940 {
00941   double dot, dot1, dot2, dot3, min, max;
00942   
00943     // edge x X
00944   if (fabs(edge[1]*edge[2]) > std::numeric_limits<double>::epsilon()) {
00945     dot = fabs(edge[2]) * dims[1] + fabs(edge[1]) * dims[2];
00946     dot1 = edge[2] * ve[1] - edge[1] * ve[2];
00947     dot2 = edge[2] * v1[1] - edge[1] * v1[2];
00948     dot3 = edge[2] * v2[1] - edge[1] * v2[2];
00949     min_max_3( dot1, dot2, dot3, min, max );
00950     if (max < -dot || min > dot)
00951       return false;
00952   }
00953   
00954     // edge x Y
00955   if (fabs(edge[1]*edge[2]) > std::numeric_limits<double>::epsilon()) {
00956     dot = fabs(edge[2]) * dims[0] + fabs(edge[0]) * dims[2];
00957     dot1 = -edge[2] * ve[0] + edge[0] * ve[2];
00958     dot2 = -edge[2] * v1[0] + edge[0] * v1[2];
00959     dot3 = -edge[2] * v2[0] + edge[0] * v2[2];
00960     min_max_3( dot1, dot2, dot3, min, max );
00961     if (max < -dot || min > dot)
00962       return false;
00963   }
00964   
00965     // edge x Z
00966   if (fabs(edge[1]*edge[2]) > std::numeric_limits<double>::epsilon()) {
00967     dot = fabs(edge[1]) * dims[0] + fabs(edge[0]) * dims[1];
00968     dot1 = edge[1] * ve[0] - edge[0] * ve[1];
00969     dot2 = edge[1] * v1[0] - edge[0] * v1[1];
00970     dot3 = edge[1] * v2[0] - edge[0] * v2[1];
00971     min_max_3( dot1, dot2, dot3, min, max );
00972     if (max < -dot || min > dot)
00973       return false;
00974   }
00975 
00976   return true;
00977 }
00978 
00979 bool box_tet_overlap( const CartVect *corners_in,
00980                       const CartVect& center,
00981                       const CartVect& dims )
00982 {
00983     // Do Separating Axis Theorem:
00984     // If the element and the box overlap, then the 1D projections
00985     // onto at least one of the axes in the following three sets
00986     // must overlap (assuming convex polyhedral element).
00987     // 1) The normals of the faces of the box (the principal axes)
00988     // 2) The crossproduct of each element edge with each box edge
00989     //    (crossproduct of each edge with each principal axis)
00990     // 3) The normals of the faces of the element
00991 
00992     // Translate problem such that box center is at origin.
00993   const CartVect corners[4] = { corners_in[0] - center,
00994                                   corners_in[1] - center,
00995                                   corners_in[2] - center,
00996                                   corners_in[3] - center };
00997 
00998     // 0) Check if any vertex is within the box
00999   if (fabs(corners[0][0]) <= dims[0] &&
01000       fabs(corners[0][1]) <= dims[1] &&
01001       fabs(corners[0][2]) <= dims[2])
01002     return true;
01003   if (fabs(corners[1][0]) <= dims[0] &&
01004       fabs(corners[1][1]) <= dims[1] &&
01005       fabs(corners[1][2]) <= dims[2])
01006     return true;
01007   if (fabs(corners[2][0]) <= dims[0] &&
01008       fabs(corners[2][1]) <= dims[1] &&
01009       fabs(corners[2][2]) <= dims[2])
01010     return true;
01011   if (fabs(corners[3][0]) <= dims[0] &&
01012       fabs(corners[3][1]) <= dims[1] &&
01013       fabs(corners[3][2]) <= dims[2])
01014     return true;
01015   
01016 
01017     // 1) Check for overlap on each principal axis (box face normal)
01018     // X
01019   if (corners[0][0] < -dims[0] &&
01020       corners[1][0] < -dims[0] &&
01021       corners[2][0] < -dims[0] &&
01022       corners[3][0] < -dims[0])
01023     return false;
01024   if (corners[0][0] >  dims[0] &&
01025       corners[1][0] >  dims[0] &&
01026       corners[2][0] >  dims[0] &&
01027       corners[3][0] >  dims[0])
01028     return false;
01029     // Y
01030   if (corners[0][1] < -dims[1] &&
01031       corners[1][1] < -dims[1] &&
01032       corners[2][1] < -dims[1] &&
01033       corners[3][1] < -dims[1])
01034     return false;
01035   if (corners[0][1] >  dims[1] &&
01036       corners[1][1] >  dims[1] &&
01037       corners[2][1] >  dims[1] &&
01038       corners[3][1] >  dims[1])
01039     return false;
01040     // Z
01041   if (corners[0][2] < -dims[2] &&
01042       corners[1][2] < -dims[2] &&
01043       corners[2][2] < -dims[2] &&
01044       corners[3][2] < -dims[2])
01045     return false;
01046   if (corners[0][2] >  dims[2] &&
01047       corners[1][2] >  dims[2] &&
01048       corners[2][2] >  dims[2] &&
01049       corners[3][2] >  dims[2])
01050     return false;
01051  
01052     // 3) test element face normals
01053   CartVect norm;
01054   double dot, dot1, dot2;
01055   
01056   const CartVect v01 = corners[1] - corners[0];
01057   const CartVect v02 = corners[2] - corners[0];
01058   norm = v01 * v02;
01059   dot = dot_abs(norm, dims);
01060   dot1 = norm % corners[0];
01061   dot2 = norm % corners[3];
01062   if (dot1 > dot2)
01063     std::swap(dot1, dot2);
01064   if (dot2 < -dot || dot1 > dot)
01065     return false;
01066   
01067   const CartVect v03 = corners[3] - corners[0];
01068   norm = v03 * v01;
01069   dot = dot_abs(norm, dims);
01070   dot1 = norm % corners[0];
01071   dot2 = norm % corners[2];
01072   if (dot1 > dot2)
01073     std::swap(dot1, dot2);
01074   if (dot2 < -dot || dot1 > dot)
01075     return false;
01076   
01077   norm = v02 * v03;
01078   dot = dot_abs(norm, dims);
01079   dot1 = norm % corners[0];
01080   dot2 = norm % corners[1];
01081   if (dot1 > dot2)
01082     std::swap(dot1, dot2);
01083   if (dot2 < -dot || dot1 > dot)
01084     return false;
01085   
01086   const CartVect v12 = corners[2] - corners[1];
01087   const CartVect v13 = corners[3] - corners[1];
01088   norm = v13 * v12;
01089   dot = dot_abs(norm, dims);
01090   dot1 = norm % corners[0];
01091   dot2 = norm % corners[1];
01092   if (dot1 > dot2)
01093     std::swap(dot1, dot2);
01094   if (dot2 < -dot || dot1 > dot)
01095     return false;
01096   
01097 
01098     // 2) test edge-edge cross products
01099     
01100   const CartVect v23 = corners[3] - corners[2];
01101   return box_tet_overlap_edge( dims, v01, corners[0], corners[2], corners[3] )
01102       && box_tet_overlap_edge( dims, v02, corners[0], corners[1], corners[3] )
01103       && box_tet_overlap_edge( dims, v03, corners[0], corners[1], corners[2] )
01104       && box_tet_overlap_edge( dims, v12, corners[1], corners[0], corners[3] )
01105       && box_tet_overlap_edge( dims, v13, corners[1], corners[0], corners[2] )
01106       && box_tet_overlap_edge( dims, v23, corners[2], corners[0], corners[1] );
01107 }
01108     
01109 
01110 
01111 
01112 //from: http://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf#search=%22closest%20point%20on%20triangle%22
01113 /*       t
01114  *   \(2)^
01115  *    \  |
01116  *     \ |
01117  *      \|
01118  *       \
01119  *       |\
01120  *       | \
01121  *       |  \  (1)
01122  *  (3)  tv  \
01123  *       |    \
01124  *       | (0) \
01125  *       |      \
01126  *-------+---sv--\----> s
01127  *       |        \ (6)
01128  *  (4)  |   (5)   \
01129  */
01130 // Worst case is either 61 flops and 5 compares or 53 flops and 6 compares,
01131 // depending on relative costs.  For all paths that do not return one of the
01132 // corner vertices, exactly one of the flops is a divide.
01133 void closest_location_on_tri( const CartVect& location,
01134                               const CartVect* vertices,
01135                               CartVect& closest_out )
01136 {                                                     // ops      comparisons
01137   const CartVect sv( vertices[1] - vertices[0] );   // +3 = 3
01138   const CartVect tv( vertices[2] - vertices[0] );   // +3 = 6
01139   const CartVect pv( vertices[0] - location );      // +3 = 9
01140   const double ss = sv % sv;                          // +5 = 14
01141   const double st = sv % tv;                          // +5 = 19
01142   const double tt = tv % tv;                          // +5 = 24
01143   const double sp = sv % pv;                          // +5 = 29
01144   const double tp = tv % pv;                          // +5 = 34
01145   const double det = ss*tt - st*st;                   // +3 = 37
01146   double s = st*tp - tt*sp;                           // +3 = 40
01147   double t = st*sp - ss*tp;                           // +3 = 43
01148   if (s+t < det) {                                    // +1 = 44, +1 = 1
01149     if (s < 0) {                                      //          +1 = 2
01150       if (t < 0) {                                    //          +1 = 3
01151         // region 4
01152         if (sp < 0) {                                 //          +1 = 4
01153           if (-sp > ss)                               //          +1 = 5
01154             closest_out = vertices[1];                //      44       5
01155           else
01156             closest_out = vertices[0] - (sp/ss) * sv; // +7 = 51,      5
01157         }
01158         else if (tp < 0) {                            //          +1 = 5
01159           if (-tp > tt)                               //          +1 = 6
01160             closest_out = vertices[2];                //      44,      6
01161           else
01162             closest_out = vertices[0] - (tp/tt) * tv; // +7 = 51,      6
01163         }
01164         else {
01165           closest_out = vertices[0];                  //      44,      5
01166         }
01167       }
01168       else {
01169         // region 3
01170         if (tp >= 0)                                  //          +1 = 4
01171           closest_out = vertices[0];                  //      44,      4
01172         else if (-tp >= tt)                           //          +1 = 5
01173           closest_out = vertices[2];                  //      44,      5
01174         else
01175           closest_out = vertices[0] - (tp/tt) * tv;   // +7 = 51,      5
01176       }
01177     }
01178     else if (t < 0) {                                 //          +1 = 3
01179       // region 5;
01180       if (sp >= 0.0)                                  //          +1 = 4
01181         closest_out = vertices[0];                    //      44,      4
01182       else if (-sp >= ss)                             //          +1 = 5
01183         closest_out = vertices[1];                    //      44       5
01184       else
01185         closest_out = vertices[0] - (sp/ss) * sv;     // +7 = 51,      5
01186     }
01187     else {
01188       // region 0
01189       const double inv_det = 1.0 / det;               // +1 = 45
01190       s *= inv_det;                                   // +1 = 46
01191       t *= inv_det;                                   // +1 = 47
01192       closest_out = vertices[0] + s*sv + t*tv;        //+12 = 59,      3  
01193     }
01194   }
01195   else {
01196     if (s < 0) {                                      //          +1 = 2
01197       // region 2
01198       s = st + sp;                                    // +1 = 45
01199       t = tt + tp;                                    // +1 = 46
01200       if (t > s) {                                    //          +1 = 3
01201         const double num = t - s;                     // +1 = 47
01202         const double den = ss - 2*st + tt;            // +3 = 50
01203         if (num > den)                                //          +1 = 4
01204           closest_out = vertices[1];                  //      50,      4
01205         else {
01206           s = num/den;                                // +1 = 51
01207           t = 1 - s;                                  // +1 = 52
01208           closest_out = s*vertices[1] + t*vertices[2];// +9 = 61,      4
01209         }
01210       }
01211       else if (t <= 0)                                //          +1 = 4
01212         closest_out = vertices[2];                    //      46,      4
01213       else if (tp >= 0)                               //          +1 = 5
01214         closest_out = vertices[0];                    //      46,      5
01215       else
01216         closest_out = vertices[0] - (tp/tt) * tv;     // +7 = 53,      5
01217     }
01218     else if (t < 0) {                                 //          +1 = 3
01219       // region 6
01220       t = st + tp;                                    // +1 = 45
01221       s = ss + sp;                                    // +1 = 46
01222       if (s > t) {                                    //          +1 = 4
01223         const double num = t - s;                     // +1 = 47
01224         const double den = tt - 2*st + ss;            // +3 = 50
01225         if (num > den)                                //          +1 = 5
01226           closest_out = vertices[2];                  //      50,      5
01227         else {
01228           t = num/den;                                // +1 = 51
01229           s = 1 - t;                                  // +1 = 52
01230           closest_out = s*vertices[1] + t*vertices[2];// +9 = 61,      5
01231         }
01232       }
01233       else if (s <= 0)                                //          +1 = 5
01234         closest_out = vertices[1];                    //      46,      5
01235       else if (sp >= 0)                               //          +1 = 6
01236         closest_out = vertices[0];                    //      46,      6
01237       else
01238         closest_out = vertices[0] - (sp/ss) * sv;     // +7 = 53,      6
01239     }
01240     else {
01241       // region 1
01242       const double num = tt + tp - st - sp;           // +3 = 47
01243       if (num <= 0) {                                 //          +1 = 4
01244         closest_out = vertices[2];                    //      47,      4
01245       }
01246       else {
01247         const double den = ss - 2*st + tt;            // +3 = 50
01248         if (num >= den)                               //          +1 = 5
01249           closest_out = vertices[1];                  //      50,      5
01250         else {
01251           s = num/den;                                // +1 = 51
01252           t = 1 - s;                                  // +1 = 52
01253           closest_out = s*vertices[1] + t*vertices[2];// +9 = 61,      5
01254         }
01255       }
01256     }
01257   }
01258 }
01259 
01260 void closest_location_on_tri( const CartVect& location,
01261                               const CartVect* vertices,
01262                               double tolerance,
01263                               CartVect& closest_out,
01264                               int& closest_topo )
01265 {
01266   const double tsqr = tolerance*tolerance;
01267   int i;
01268   CartVect pv[3], ev, ep;
01269   double t;
01270 
01271   closest_location_on_tri( location, vertices, closest_out );
01272   
01273   for (i = 0; i < 3; ++i) {
01274     pv[i] = vertices[i] - closest_out;
01275     if ((pv[i] % pv[i]) <= tsqr) {
01276       closest_topo = i;
01277       return;
01278     }
01279   }
01280   
01281   for (i = 0; i < 3; ++i) {
01282     ev = vertices[(i+1)%3] - vertices[i];
01283     t = (ev % pv[i]) / (ev % ev);
01284     ep = closest_out - (vertices[i] + t * ev);
01285     if ((ep % ep) <= tsqr) {
01286       closest_topo = i+3;
01287       return;
01288     }
01289   }
01290   
01291   closest_topo = 6;
01292 }
01293  
01294     
01295 // We assume polygon is *convex*, but *not* planar.
01296 void closest_location_on_polygon( const CartVect& location,
01297                                   const CartVect* vertices,
01298                                   int num_vertices,
01299                                   CartVect& closest_out )
01300 {
01301   const int n = num_vertices;
01302   CartVect d, p, v;
01303   double shortest_sqr, dist_sqr, t_closest, t;
01304   int i, e;
01305   
01306     // Find closest edge of polygon.
01307   e = n - 1;
01308   v = vertices[0] - vertices[e];
01309   t_closest = (v % (location - vertices[e])) / (v % v);
01310   if (t_closest < 0.0)
01311     d = location - vertices[e];
01312   else if (t_closest > 1.0)
01313     d = location - vertices[0];
01314   else 
01315     d = location - vertices[e] - t_closest * v;
01316   shortest_sqr = d % d;
01317   for (i = 0; i < n - 1; ++i) {
01318     v = vertices[i+1] - vertices[i];
01319     t = (v % (location - vertices[i])) / (v % v);
01320     if (t < 0.0)
01321       d = location - vertices[i];
01322     else if (t > 1.0)
01323       d = location - vertices[i+1];
01324     else
01325       d = location - vertices[i] - t * v;
01326     dist_sqr = d % d;
01327     if (dist_sqr < shortest_sqr) {
01328       e = i;
01329       shortest_sqr = dist_sqr;
01330       t_closest = t;
01331     }
01332   }
01333   
01334     // If we are beyond the bounds of the edge, then
01335     // the point is outside and closest to a vertex
01336   if (t_closest <= 0.0) {
01337     closest_out = vertices[e];
01338     return;
01339   }
01340   else if (t_closest >= 1.0) {
01341     closest_out = vertices[(e+1)%n];
01342     return;
01343   }
01344   
01345     // Now check which side of the edge we are one
01346   const CartVect v0 = vertices[e] - vertices[(e+n-1)%n];
01347   const CartVect v1 = vertices[(e+1)%n] - vertices[e];
01348   const CartVect v2 = vertices[(e+2)%n] - vertices[(e+1)%n];
01349   const CartVect norm = (1.0 - t_closest) * (v0 * v1) + t_closest * (v1 * v2);
01350     // if on outside of edge, result is closest point on edge
01351   if ((norm % ((vertices[e] - location) * v1)) <= 0.0) { 
01352     closest_out = vertices[e] + t_closest * v1;
01353     return;
01354   }
01355   
01356     // Inside.  Project to plane defined by point and normal at
01357     // closest edge
01358   const double D = -(norm % (vertices[e] + t_closest * v1));
01359   closest_out = (location - (norm % location + D) * norm)/(norm % norm);
01360 }
01361 
01362 void closest_location_on_box( const CartVect& min,
01363                               const CartVect& max,
01364                               const CartVect& point,
01365                               CartVect& closest )
01366 {
01367   closest[0] = point[0] < min[0] ? min[0] : point[0] > max[0] ? max[0] : point[0];
01368   closest[1] = point[1] < min[1] ? min[1] : point[1] > max[1] ? max[1] : point[1];
01369   closest[2] = point[2] < min[2] ? min[2] : point[2] > max[2] ? max[2] : point[2];
01370 }
01371 
01372 bool box_point_overlap( const CartVect& box_min_corner,
01373                         const CartVect& box_max_corner,
01374                         const CartVect& point,
01375                         double tolerance )
01376 {
01377   CartVect closest;
01378   closest_location_on_box( box_min_corner, box_max_corner, point, closest );
01379   closest -= point;
01380   return closest % closest < tolerance * tolerance;
01381 }
01382 
01383 bool boxes_overlap( const CartVect & box_min1, const CartVect & box_max1,
01384     const CartVect & box_min2, const CartVect & box_max2, double tolerance)
01385 {
01386 
01387   for (int k=0; k<3; k++)
01388   {
01389     double b1min=box_min1[k], b1max=box_max1[k];
01390     double b2min=box_min2[k], b2max=box_max2[k];
01391     if ( b1min - tolerance > b2max)
01392       return false;
01393     if (b2min - tolerance > b1max )
01394       return false;
01395   }
01396   return true;
01397 }
01398 
01399 // see if boxes formed by 2 lists of "CartVect"s overlap
01400 bool bounding_boxes_overlap (const CartVect * list1, int num1, const CartVect * list2, int num2,
01401       double tolerance)
01402 {
01403   assert(num1>=1 && num2>=1);
01404   CartVect box_min1=list1[0], box_max1=list1[0];
01405   CartVect box_min2=list2[0], box_max2=list2[0];
01406   for (int i=1; i<num1; i++)
01407   {
01408     for (int k=0; k<3; k++)
01409     {
01410       double val=list1[i][k];
01411       if (box_min1[k] > val)
01412         box_min1[k] = val;
01413       if (box_max1[k] < val)
01414         box_max1[k]=val;
01415     }
01416   }
01417   for (int i=1; i<num2; i++)
01418   {
01419     for (int k=0; k<3; k++)
01420     {
01421       double val=list2[i][k];
01422       if (box_min2[k] > val)
01423         box_min2[k] = val;
01424       if (box_max2[k] < val)
01425         box_max2[k]=val;
01426     }
01427   }
01428 
01429   return boxes_overlap(box_min1, box_max1, box_min2, box_max2, tolerance);
01430 }
01432 class VolMap {
01433   public:
01435     virtual CartVect center_xi() const = 0;
01437     virtual CartVect evaluate( const CartVect& xi ) const = 0;
01439     virtual Matrix3 jacobian( const CartVect& xi ) const = 0;
01441     bool solve_inverse( const CartVect& x, CartVect& xi, double tol ) const ;
01442 };
01443 
01444 bool VolMap::solve_inverse( const CartVect& x, CartVect& xi, double tol ) const
01445 {
01446   const double error_tol_sqr = tol*tol;
01447   double det;
01448   xi = center_xi();
01449   CartVect delta = evaluate(xi) - x;
01450   Matrix3 J;
01451   while (delta % delta > error_tol_sqr) {
01452     J = jacobian(xi);
01453     det = J.determinant();
01454     if (det < std::numeric_limits<double>::epsilon())
01455       return false;
01456     xi -= J.inverse(1.0/det) * delta;
01457     delta = evaluate( xi ) - x;
01458   }
01459   return true;
01460 }
01461 
01463 class LinearHexMap : public VolMap {
01464   public:
01465     LinearHexMap( const CartVect* corner_coords ) : corners(corner_coords) {}
01466     virtual CartVect center_xi() const;
01467     virtual CartVect evaluate( const CartVect& xi ) const;
01468     virtual Matrix3 jacobian( const CartVect& xi ) const;
01469   private:
01470     const CartVect* corners;
01471     static const double corner_xi[8][3];
01472 };
01473 
01474 const double LinearHexMap::corner_xi[8][3] = { { -1, -1, -1 },
01475                                                {  1, -1, -1 },
01476                                                {  1,  1, -1 },
01477                                                { -1,  1, -1 },
01478                                                { -1, -1,  1 },
01479                                                {  1, -1,  1 },
01480                                                {  1,  1,  1 },
01481                                                { -1,  1,  1 } };
01482 CartVect LinearHexMap::center_xi() const
01483   { return CartVect(0.0); }
01484 
01485 CartVect LinearHexMap::evaluate( const CartVect& xi ) const
01486 {
01487   CartVect x(0.0);
01488   for (unsigned i = 0; i < 8; ++i) {
01489     const double N_i = (1 + xi[0]*corner_xi[i][0])
01490                      * (1 + xi[1]*corner_xi[i][1])
01491                      * (1 + xi[2]*corner_xi[i][2]);
01492     x += N_i * corners[i];
01493   }
01494   x *= 0.125;
01495   return x;
01496 }
01497 
01498 Matrix3 LinearHexMap::jacobian( const CartVect& xi ) const
01499 {
01500   Matrix3 J(0.0);
01501   for (unsigned i = 0; i < 8; ++i) {
01502     const double   xi_p = 1 + xi[0]*corner_xi[i][0];
01503     const double  eta_p = 1 + xi[1]*corner_xi[i][1];
01504     const double zeta_p = 1 + xi[2]*corner_xi[i][2];
01505     const double dNi_dxi   = corner_xi[i][0] * eta_p * zeta_p;
01506     const double dNi_deta  = corner_xi[i][1] *  xi_p * zeta_p;
01507     const double dNi_dzeta = corner_xi[i][2] *  xi_p *  eta_p;
01508     J(0,0) += dNi_dxi   * corners[i][0];
01509     J(1,0) += dNi_dxi   * corners[i][1];
01510     J(2,0) += dNi_dxi   * corners[i][2];
01511     J(0,1) += dNi_deta  * corners[i][0];
01512     J(1,1) += dNi_deta  * corners[i][1];
01513     J(2,1) += dNi_deta  * corners[i][2];
01514     J(0,2) += dNi_dzeta * corners[i][0];
01515     J(1,2) += dNi_dzeta * corners[i][1];
01516     J(2,2) += dNi_dzeta * corners[i][2];
01517   }
01518   return J *= 0.125;
01519 }
01520 
01521 bool nat_coords_trilinear_hex( const CartVect* corner_coords,
01522                                const CartVect& x,
01523                                CartVect& xi,
01524                                double tol )
01525 {
01526   return LinearHexMap( corner_coords ).solve_inverse( x, xi, tol );
01527 }
01528 
01529 
01530 bool point_in_trilinear_hex(const CartVect *hex, 
01531                             const CartVect& xyz,
01532                             double etol) 
01533 {
01534   CartVect xi;
01535   return nat_coords_trilinear_hex( hex, xyz, xi, etol )
01536       && fabs(xi[0])-1 < etol 
01537       && fabs(xi[1])-1 < etol 
01538       && fabs(xi[2])-1 < etol;
01539 }
01540 
01541 
01542 
01543 } // namespace GeomUtil
01544   
01545 } // namespace moab
01546 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines