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