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