MOAB: Mesh Oriented datABase
(version 5.2.1)
|
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 <assert.h> 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