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