 |
Mesh Oriented datABase
(version 5.5.1)
An array-based unstructured mesh library
|
|
Mesh Oriented datABase
(version 5.5.1)
An array-based unstructured mesh library
|
1 /*
2 * MOAB, a Mesh-Oriented datABase, is a software component for creating,
3 * storing and accessing finite element mesh data.
4 *
5 * Copyright 2004 Sandia Corporation. Under the terms of Contract
6 * DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government
7 * retains certain rights in this software.
8 *
9 * This library is free software; you can redistribute it and/or
10 * modify it under the terms of the GNU Lesser General Public
11 * License as published by the Free Software Foundation; either
12 * version 2.1 of the License, or (at your option) any later version.
13 *
14 */
15
16 /**\file Geometry.cpp
17 *\author Jason Kraftcheck (kraftche@cae.wisc.edu)
18 *\date 2006-07-27
19 */
20
21 #include "moab/CartVect.hpp"
22 #include "moab/CN.hpp"
23 #include "moab/GeomUtil.hpp"
24 #include "moab/Matrix3.hpp"
25 #include "moab/Util.hpp"
26 #include <cmath>
27 #include <algorithm>
28 #include <cassert>
29 #include <iostream>
30 #include <limits>
31
32 namespace moab
33 {
34
35 namespace GeomUtil
36 {
37
38 static inline void min_max_3( double a, double b, double c, double& min, double& max )
39 {
40 if( a < b )
41 {
42 if( a < c )
43 {
44 min = a;
45 max = b > c ? b : c;
46 }
47 else
48 {
49 min = c;
50 max = b;
51 }
52 }
53 else if( b < c )
54 {
55 min = b;
56 max = a > c ? a : c;
57 }
58 else
59 {
60 min = c;
61 max = a;
62 }
63 }
64
65 static inline double dot_abs( const CartVect& u, const CartVect& v )
66 {
67 return fabs( u[0] * v[0] ) + fabs( u[1] * v[1] ) + fabs( u[2] * v[2] );
68 }
69
70 bool segment_box_intersect( CartVect box_min,
71 CartVect box_max,
72 const CartVect& seg_pt,
73 const CartVect& seg_unit_dir,
74 double& seg_start,
75 double& seg_end )
76 {
77 // translate so that seg_pt is at origin
78 box_min -= seg_pt;
79 box_max -= seg_pt;
80
81 for( unsigned i = 0; i < 3; ++i )
82 { // X, Y, and Z slabs
83
84 // intersect line with slab planes
85 const double t_min = box_min[i] / seg_unit_dir[i];
86 const double t_max = box_max[i] / seg_unit_dir[i];
87
88 // check if line is parallel to planes
89 if( !Util::is_finite( t_min ) )
90 {
91 if( box_min[i] > 0.0 || box_max[i] < 0.0 ) return false;
92 continue;
93 }
94
95 if( seg_unit_dir[i] < 0 )
96 {
97 if( t_min < seg_end ) seg_end = t_min;
98 if( t_max > seg_start ) seg_start = t_max;
99 }
100 else
101 { // seg_unit_dir[i] > 0
102 if( t_min > seg_start ) seg_start = t_min;
103 if( t_max < seg_end ) seg_end = t_max;
104 }
105 }
106
107 return seg_start <= seg_end;
108 }
109
110 /* Function to return the vertex with the lowest coordinates. To force the same
111 ray-edge computation, the Plücker test needs to use consistent edge
112 representation. This would be more simple with MOAB handles instead of
113 coordinates... */
114 inline bool first( const CartVect& a, const CartVect& b )
115 {
116 if( a[0] < b[0] )
117 {
118 return true;
119 }
120 else if( a[0] == b[0] )
121 {
122 if( a[1] < b[1] )
123 {
124 return true;
125 }
126 else if( a[1] == b[1] )
127 {
128 if( a[2] < b[2] )
129 {
130 return true;
131 }
132 else
133 {
134 return false;
135 }
136 }
137 else
138 {
139 return false;
140 }
141 }
142 else
143 {
144 return false;
145 }
146 }
147
148 double plucker_edge_test( const CartVect& vertexa,
149 const CartVect& vertexb,
150 const CartVect& ray,
151 const CartVect& ray_normal )
152 {
153
154 double pip;
155 const double near_zero = 10 * std::numeric_limits< double >::epsilon();
156
157 if( first( vertexa, vertexb ) )
158 {
159 const CartVect edge = vertexb - vertexa;
160 const CartVect edge_normal = edge * vertexa;
161 pip = ray % edge_normal + ray_normal % edge;
162 }
163 else
164 {
165 const CartVect edge = vertexa - vertexb;
166 const CartVect edge_normal = edge * vertexb;
167 pip = ray % edge_normal + ray_normal % edge;
168 pip = -pip;
169 }
170
171 if( near_zero > fabs( pip ) ) pip = 0.0;
172
173 return pip;
174 }
175
176 #define EXIT_EARLY \
177 if( type ) *type = NONE; \
178 return false;
179
180 /* This test uses the same edge-ray computation for adjacent triangles so that
181 rays passing close to edges/nodes are handled consistently.
182
183 Reports intersection type for post processing of special cases. Optionally
184 screen by orientation and negative/nonnegative distance limits.
185
186 If screening by orientation, substantial pruning can occur. Indicate
187 desired orientation by passing 1 (forward), -1 (reverse), or 0 (no preference).
188 Note that triangle orientation is not always the same as surface
189 orientation due to non-manifold surfaces.
190
191 N. Platis and T. Theoharis, "Fast Ray-Tetrahedron Intersection using Plücker
192 Coordinates", Journal of Graphics Tools, Vol. 8, Part 4, Pages 37-48 (2003). */
193 bool plucker_ray_tri_intersect( const CartVect vertices[3],
194 const CartVect& origin,
195 const CartVect& direction,
196 double& dist_out,
197 const double* nonneg_ray_len,
198 const double* neg_ray_len,
199 const int* orientation,
200 intersection_type* type )
201 {
202
203 const CartVect raya = direction;
204 const CartVect rayb = direction * origin;
205
206 // Determine the value of the first Plucker coordinate from edge 0
207 double plucker_coord0 = plucker_edge_test( vertices[0], vertices[1], raya, rayb );
208
209 // If orientation is set, confirm that sign of plucker_coordinate indicate
210 // correct orientation of intersection
211 if( orientation && ( *orientation ) * plucker_coord0 > 0 )
212 {
213 EXIT_EARLY
214 }
215
216 // Determine the value of the second Plucker coordinate from edge 1
217 double plucker_coord1 = plucker_edge_test( vertices[1], vertices[2], raya, rayb );
218
219 // If orientation is set, confirm that sign of plucker_coordinate indicate
220 // correct orientation of intersection
221 if( orientation )
222 {
223 if( ( *orientation ) * plucker_coord1 > 0 )
224 {
225 EXIT_EARLY
226 }
227 // If the orientation is not specified, all plucker_coords must be the same sign or
228 // zero.
229 }
230 else if( ( 0.0 < plucker_coord0 && 0.0 > plucker_coord1 ) || ( 0.0 > plucker_coord0 && 0.0 < plucker_coord1 ) )
231 {
232 EXIT_EARLY
233 }
234
235 // Determine the value of the second Plucker coordinate from edge 2
236 double plucker_coord2 = plucker_edge_test( vertices[2], vertices[0], raya, rayb );
237
238 // If orientation is set, confirm that sign of plucker_coordinate indicate
239 // correct orientation of intersection
240 if( orientation )
241 {
242 if( ( *orientation ) * plucker_coord2 > 0 )
243 {
244 EXIT_EARLY
245 }
246 // If the orientation is not specified, all plucker_coords must be the same sign or
247 // zero.
248 }
249 else if( ( 0.0 < plucker_coord1 && 0.0 > plucker_coord2 ) || ( 0.0 > plucker_coord1 && 0.0 < plucker_coord2 ) ||
250 ( 0.0 < plucker_coord0 && 0.0 > plucker_coord2 ) || ( 0.0 > plucker_coord0 && 0.0 < plucker_coord2 ) )
251 {
252 EXIT_EARLY
253 }
254
255 // check for coplanar case to avoid dividing by zero
256 if( 0.0 == plucker_coord0 && 0.0 == plucker_coord1 && 0.0 == plucker_coord2 )
257 {
258 EXIT_EARLY
259 }
260
261 // get the distance to intersection
262 const double inverse_sum = 1.0 / ( plucker_coord0 + plucker_coord1 + plucker_coord2 );
263 assert( 0.0 != inverse_sum );
264 const CartVect intersection( plucker_coord0 * inverse_sum * vertices[2] +
265 plucker_coord1 * inverse_sum * vertices[0] +
266 plucker_coord2 * inverse_sum * vertices[1] );
267
268 // To minimize numerical error, get index of largest magnitude direction.
269 int idx = 0;
270 double max_abs_dir = 0;
271 for( unsigned int i = 0; i < 3; ++i )
272 {
273 if( fabs( direction[i] ) > max_abs_dir )
274 {
275 idx = i;
276 max_abs_dir = fabs( direction[i] );
277 }
278 }
279 const double dist = ( intersection[idx] - origin[idx] ) / direction[idx];
280
281 // is the intersection within distance limits?
282 if( ( nonneg_ray_len && *nonneg_ray_len < dist ) || // intersection is beyond positive limit
283 ( neg_ray_len && *neg_ray_len >= dist ) || // intersection is behind negative limit
284 ( !neg_ray_len && 0 > dist ) )
285 { // Unless a neg_ray_len is used, don't return negative distances
286 EXIT_EARLY
287 }
288
289 dist_out = dist;
290
291 if( type )
292 *type = type_list[( ( 0.0 == plucker_coord2 ) << 2 ) + ( ( 0.0 == plucker_coord1 ) << 1 ) +
293 ( 0.0 == plucker_coord0 )];
294
295 return true;
296 }
297
298 /* Implementation copied from cgmMC ray_tri_contact (overlap.C) */
299 bool ray_tri_intersect( const CartVect vertices[3],
300 const CartVect& b,
301 const CartVect& v,
302 double& t_out,
303 const double* ray_length )
304 {
305 const CartVect p0 = vertices[0] - vertices[1]; // abc
306 const CartVect p1 = vertices[0] - vertices[2]; // def
307 // ghi<-v
308 const CartVect p = vertices[0] - b; // jkl
309 const CartVect c = p1 * v; // eiMinushf,gfMinusdi,dhMinuseg
310 const double mP = p0 % c;
311 const double betaP = p % c;
312 if( mP > 0 )
313 {
314 if( betaP < 0 ) return false;
315 }
316 else if( mP < 0 )
317 {
318 if( betaP > 0 ) return false;
319 }
320 else
321 {
322 return false;
323 }
324
325 const CartVect d = p0 * p; // jcMinusal,blMinuskc,akMinusjb
326 double gammaP = v % d;
327 if( mP > 0 )
328 {
329 if( gammaP < 0 || betaP + gammaP > mP ) return false;
330 }
331 else if( betaP + gammaP < mP || gammaP > 0 )
332 return false;
333
334 const double tP = p1 % d;
335 const double m = 1.0 / mP;
336 const double beta = betaP * m;
337 const double gamma = gammaP * m;
338 const double t = -tP * m;
339 if( ray_length && t > *ray_length ) return false;
340
341 if( beta < 0 || gamma < 0 || beta + gamma > 1 || t < 0.0 ) return false;
342
343 t_out = t;
344 return true;
345 }
346
347 bool ray_box_intersect( const CartVect& box_min,
348 const CartVect& box_max,
349 const CartVect& ray_pt,
350 const CartVect& ray_dir,
351 double& t_enter,
352 double& t_exit )
353 {
354 const double epsilon = 1e-12;
355 double t1, t2;
356
357 // Use 'slabs' method from 13.6.1 of Akenine-Moller
358 t_enter = 0.0;
359 t_exit = std::numeric_limits< double >::infinity();
360
361 // Intersect with each pair of axis-aligned planes bounding
362 // opposite faces of the leaf box
363 bool ray_is_valid = false; // is ray direction vector zero?
364 for( int axis = 0; axis < 3; ++axis )
365 {
366 if( fabs( ray_dir[axis] ) < epsilon )
367 { // ray parallel to planes
368 if( ray_pt[axis] >= box_min[axis] && ray_pt[axis] <= box_max[axis] )
369 continue;
370 else
371 return false;
372 }
373
374 // find t values at which ray intersects each plane
375 ray_is_valid = true;
376 t1 = ( box_min[axis] - ray_pt[axis] ) / ray_dir[axis];
377 t2 = ( box_max[axis] - ray_pt[axis] ) / ray_dir[axis];
378
379 // t_enter = max( t_enter_x, t_enter_y, t_enter_z )
380 // t_exit = min( t_exit_x, t_exit_y, t_exit_z )
381 // where
382 // t_enter_x = min( t1_x, t2_x );
383 // t_exit_x = max( t1_x, t2_x )
384 if( t1 < t2 )
385 {
386 if( t_enter < t1 ) t_enter = t1;
387 if( t_exit > t2 ) t_exit = t2;
388 }
389 else
390 {
391 if( t_enter < t2 ) t_enter = t2;
392 if( t_exit > t1 ) t_exit = t1;
393 }
394 }
395
396 return ray_is_valid && ( t_enter <= t_exit );
397 }
398
399 bool box_plane_overlap( const CartVect& normal, double d, CartVect min, CartVect max )
400 {
401 if( normal[0] < 0.0 ) std::swap( min[0], max[0] );
402 if( normal[1] < 0.0 ) std::swap( min[1], max[1] );
403 if( normal[2] < 0.0 ) std::swap( min[2], max[2] );
404
405 return ( normal % min <= -d ) && ( normal % max >= -d );
406 }
407
408 #define CHECK_RANGE( A, B, R ) \
409 if( ( A ) < ( B ) ) \
410 { \
411 if( ( A ) > ( R ) || ( B ) < -( R ) ) return false; \
412 } \
413 else if( ( B ) > ( R ) || ( A ) < -( R ) ) \
414 return false
415
416 /* Adapted from: http://jgt.akpeters.com/papers/AkenineMoller01/tribox.html
417 * Use separating axis theorem to test for overlap between triangle
418 * and axis-aligned box.
419 *
420 * Test for overlap in these directions:
421 * 1) {x,y,z}-directions
422 * 2) normal of triangle
423 * 3) crossprod of triangle edge with {x,y,z}-direction
424 */
425 bool box_tri_overlap( const CartVect vertices[3], const CartVect& box_center, const CartVect& box_dims )
426 {
427 // translate everything such that box is centered at origin
428 const CartVect v0( vertices[0] - box_center );
429 const CartVect v1( vertices[1] - box_center );
430 const CartVect v2( vertices[2] - box_center );
431
432 // do case 1) tests
433 if( v0[0] > box_dims[0] && v1[0] > box_dims[0] && v2[0] > box_dims[0] ) return false;
434 if( v0[1] > box_dims[1] && v1[1] > box_dims[1] && v2[1] > box_dims[1] ) return false;
435 if( v0[2] > box_dims[2] && v1[2] > box_dims[2] && v2[2] > box_dims[2] ) return false;
436 if( v0[0] < -box_dims[0] && v1[0] < -box_dims[0] && v2[0] < -box_dims[0] ) return false;
437 if( v0[1] < -box_dims[1] && v1[1] < -box_dims[1] && v2[1] < -box_dims[1] ) return false;
438 if( v0[2] < -box_dims[2] && v1[2] < -box_dims[2] && v2[2] < -box_dims[2] ) return false;
439
440 // compute triangle edge vectors
441 const CartVect e0( vertices[1] - vertices[0] );
442 const CartVect e1( vertices[2] - vertices[1] );
443 const CartVect e2( vertices[0] - vertices[2] );
444
445 // do case 3) tests
446 double fex, fey, fez, p0, p1, p2, rad;
447 fex = fabs( e0[0] );
448 fey = fabs( e0[1] );
449 fez = fabs( e0[2] );
450
451 p0 = e0[2] * v0[1] - e0[1] * v0[2];
452 p2 = e0[2] * v2[1] - e0[1] * v2[2];
453 rad = fez * box_dims[1] + fey * box_dims[2];
454 CHECK_RANGE( p0, p2, rad );
455
456 p0 = -e0[2] * v0[0] + e0[0] * v0[2];
457 p2 = -e0[2] * v2[0] + e0[0] * v2[2];
458 rad = fez * box_dims[0] + fex * box_dims[2];
459 CHECK_RANGE( p0, p2, rad );
460
461 p1 = e0[1] * v1[0] - e0[0] * v1[1];
462 p2 = e0[1] * v2[0] - e0[0] * v2[1];
463 rad = fey * box_dims[0] + fex * box_dims[1];
464 CHECK_RANGE( p1, p2, rad );
465
466 fex = fabs( e1[0] );
467 fey = fabs( e1[1] );
468 fez = fabs( e1[2] );
469
470 p0 = e1[2] * v0[1] - e1[1] * v0[2];
471 p2 = e1[2] * v2[1] - e1[1] * v2[2];
472 rad = fez * box_dims[1] + fey * box_dims[2];
473 CHECK_RANGE( p0, p2, rad );
474
475 p0 = -e1[2] * v0[0] + e1[0] * v0[2];
476 p2 = -e1[2] * v2[0] + e1[0] * v2[2];
477 rad = fez * box_dims[0] + fex * box_dims[2];
478 CHECK_RANGE( p0, p2, rad );
479
480 p0 = e1[1] * v0[0] - e1[0] * v0[1];
481 p1 = e1[1] * v1[0] - e1[0] * v1[1];
482 rad = fey * box_dims[0] + fex * box_dims[1];
483 CHECK_RANGE( p0, p1, rad );
484
485 fex = fabs( e2[0] );
486 fey = fabs( e2[1] );
487 fez = fabs( e2[2] );
488
489 p0 = e2[2] * v0[1] - e2[1] * v0[2];
490 p1 = e2[2] * v1[1] - e2[1] * v1[2];
491 rad = fez * box_dims[1] + fey * box_dims[2];
492 CHECK_RANGE( p0, p1, rad );
493
494 p0 = -e2[2] * v0[0] + e2[0] * v0[2];
495 p1 = -e2[2] * v1[0] + e2[0] * v1[2];
496 rad = fez * box_dims[0] + fex * box_dims[2];
497 CHECK_RANGE( p0, p1, rad );
498
499 p1 = e2[1] * v1[0] - e2[0] * v1[1];
500 p2 = e2[1] * v2[0] - e2[0] * v2[1];
501 rad = fey * box_dims[0] + fex * box_dims[1];
502 CHECK_RANGE( p1, p2, rad );
503
504 // do case 2) test
505 CartVect n = e0 * e1;
506 return box_plane_overlap( n, -( n % v0 ), -box_dims, box_dims );
507 }
508
509 bool box_tri_overlap( const CartVect triangle_corners[3],
510 const CartVect& box_min_corner,
511 const CartVect& box_max_corner,
512 double tolerance )
513 {
514 const CartVect box_center = 0.5 * ( box_max_corner + box_min_corner );
515 const CartVect box_hf_dim = 0.5 * ( box_max_corner - box_min_corner );
516 return box_tri_overlap( triangle_corners, box_center, box_hf_dim + CartVect( tolerance ) );
517 }
518
519 bool box_elem_overlap( const CartVect* elem_corners,
520 EntityType elem_type,
521 const CartVect& center,
522 const CartVect& dims,
523 int nodecount )
524 {
525
526 switch( elem_type )
527 {
528 case MBTRI:
529 return box_tri_overlap( elem_corners, center, dims );
530 case MBTET:
531 return box_tet_overlap( elem_corners, center, dims );
532 case MBHEX:
533 return box_hex_overlap( elem_corners, center, dims );
534 case MBPOLYGON: {
535 CartVect vt[3];
536 vt[0] = elem_corners[0];
537 vt[1] = elem_corners[1];
538 for( int j = 2; j < nodecount; j++ )
539 {
540 vt[2] = elem_corners[j];
541 if( box_tri_overlap( vt, center, dims ) ) return true;
542 }
543 // none of the triangles overlap, then we return false
544 return false;
545 }
546 case MBPOLYHEDRON:
547 assert( false );
548 return false;
549 default:
550 return box_linear_elem_overlap( elem_corners, elem_type, center, dims );
551 }
552 }
553
554 static inline CartVect quad_norm( const CartVect& v1, const CartVect& v2, const CartVect& v3, const CartVect& v4 )
555 {
556 return ( -v1 + v2 + v3 - v4 ) * ( -v1 - v2 + v3 + v4 );
557 }
558
559 static inline CartVect tri_norm( const CartVect& v1, const CartVect& v2, const CartVect& v3 )
560 {
561 return ( v2 - v1 ) * ( v3 - v1 );
562 }
563
564 bool box_linear_elem_overlap( const CartVect* elem_corners,
565 EntityType type,
566 const CartVect& box_center,
567 const CartVect& box_halfdims )
568 {
569 CartVect corners[8];
570 const unsigned num_corner = CN::VerticesPerEntity( type );
571 assert( num_corner <= sizeof( corners ) / sizeof( corners[0] ) );
572 for( unsigned i = 0; i < num_corner; ++i )
573 corners[i] = elem_corners[i] - box_center;
574 return box_linear_elem_overlap( corners, type, box_halfdims );
575 }
576
577 bool box_linear_elem_overlap( const CartVect* elem_corners, EntityType type, const CartVect& dims )
578 {
579 // Do Separating Axis Theorem:
580 // If the element and the box overlap, then the 1D projections
581 // onto at least one of the axes in the following three sets
582 // must overlap (assuming convex polyhedral element).
583 // 1) The normals of the faces of the box (the principal axes)
584 // 2) The crossproduct of each element edge with each box edge
585 // (crossproduct of each edge with each principal axis)
586 // 3) The normals of the faces of the element
587
588 int e, f; // loop counters
589 int i;
590 double dot, cross[2], tmp;
591 CartVect norm;
592 int indices[4]; // element edge/face vertex indices
593
594 // test box face normals (principal axes)
595 const int num_corner = CN::VerticesPerEntity( type );
596 int not_less[3] = { num_corner, num_corner, num_corner };
597 int not_greater[3] = { num_corner, num_corner, num_corner };
598 int not_inside;
599 for( i = 0; i < num_corner; ++i )
600 { // for each element corner
601 not_inside = 3;
602
603 if( elem_corners[i][0] < -dims[0] )
604 --not_less[0];
605 else if( elem_corners[i][0] > dims[0] )
606 --not_greater[0];
607 else
608 --not_inside;
609
610 if( elem_corners[i][1] < -dims[1] )
611 --not_less[1];
612 else if( elem_corners[i][1] > dims[1] )
613 --not_greater[1];
614 else
615 --not_inside;
616
617 if( elem_corners[i][2] < -dims[2] )
618 --not_less[2];
619 else if( elem_corners[i][2] > dims[2] )
620 --not_greater[2];
621 else
622 --not_inside;
623
624 if( !not_inside ) return true;
625 }
626 // If all points less than min_x of box, then
627 // not_less[0] == 0, and therefore
628 // the following product is zero.
629 if( not_greater[0] * not_greater[1] * not_greater[2] * not_less[0] * not_less[1] * not_less[2] == 0 )
630 return false;
631
632 // Test edge-edge crossproducts
633
634 // Edge directions for box are principal axis, so
635 // for each element edge, check along the cross-product
636 // of that edge with each of the tree principal axes.
637 const int num_edge = CN::NumSubEntities( type, 1 );
638 for( e = 0; e < num_edge; ++e )
639 { // for each element edge
640 // get which element vertices bound the edge
641 CN::SubEntityVertexIndices( type, 1, e, indices );
642
643 // X-Axis
644
645 // calculate crossproduct: axis x (v1 - v0),
646 // where v1 and v0 are edge vertices.
647 cross[0] = elem_corners[indices[0]][2] - elem_corners[indices[1]][2];
648 cross[1] = elem_corners[indices[1]][1] - elem_corners[indices[0]][1];
649 // skip if parallel
650 if( ( cross[0] * cross[0] + cross[1] * cross[1] ) >= std::numeric_limits< double >::epsilon() )
651 {
652 dot = fabs( cross[0] * dims[1] ) + fabs( cross[1] * dims[2] );
653 not_less[0] = not_greater[0] = num_corner - 1;
654 for( i = ( indices[0] + 1 ) % num_corner; i != indices[0]; i = ( i + 1 ) % num_corner )
655 { // for each element corner
656 tmp = cross[0] * elem_corners[i][1] + cross[1] * elem_corners[i][2];
657 not_less[0] -= ( tmp < -dot );
658 not_greater[0] -= ( tmp > dot );
659 }
660
661 if( not_less[0] * not_greater[0] == 0 ) return false;
662 }
663
664 // Y-Axis
665
666 // calculate crossproduct: axis x (v1 - v0),
667 // where v1 and v0 are edge vertices.
668 cross[0] = elem_corners[indices[0]][0] - elem_corners[indices[1]][0];
669 cross[1] = elem_corners[indices[1]][2] - elem_corners[indices[0]][2];
670 // skip if parallel
671 if( ( cross[0] * cross[0] + cross[1] * cross[1] ) >= std::numeric_limits< double >::epsilon() )
672 {
673 dot = fabs( cross[0] * dims[2] ) + fabs( cross[1] * dims[0] );
674 not_less[0] = not_greater[0] = num_corner - 1;
675 for( i = ( indices[0] + 1 ) % num_corner; i != indices[0]; i = ( i + 1 ) % num_corner )
676 { // for each element corner
677 tmp = cross[0] * elem_corners[i][2] + cross[1] * elem_corners[i][0];
678 not_less[0] -= ( tmp < -dot );
679 not_greater[0] -= ( tmp > dot );
680 }
681
682 if( not_less[0] * not_greater[0] == 0 ) return false;
683 }
684
685 // Z-Axis
686
687 // calculate crossproduct: axis x (v1 - v0),
688 // where v1 and v0 are edge vertices.
689 cross[0] = elem_corners[indices[0]][1] - elem_corners[indices[1]][1];
690 cross[1] = elem_corners[indices[1]][0] - elem_corners[indices[0]][0];
691 // skip if parallel
692 if( ( cross[0] * cross[0] + cross[1] * cross[1] ) >= std::numeric_limits< double >::epsilon() )
693 {
694 dot = fabs( cross[0] * dims[0] ) + fabs( cross[1] * dims[1] );
695 not_less[0] = not_greater[0] = num_corner - 1;
696 for( i = ( indices[0] + 1 ) % num_corner; i != indices[0]; i = ( i + 1 ) % num_corner )
697 { // for each element corner
698 tmp = cross[0] * elem_corners[i][0] + cross[1] * elem_corners[i][1];
699 not_less[0] -= ( tmp < -dot );
700 not_greater[0] -= ( tmp > dot );
701 }
702
703 if( not_less[0] * not_greater[0] == 0 ) return false;
704 }
705 }
706
707 // test element face normals
708 const int num_face = CN::NumSubEntities( type, 2 );
709 for( f = 0; f < num_face; ++f )
710 {
711 CN::SubEntityVertexIndices( type, 2, f, indices );
712 switch( CN::SubEntityType( type, 2, f ) )
713 {
714 case MBTRI:
715 norm = tri_norm( elem_corners[indices[0]], elem_corners[indices[1]], elem_corners[indices[2]] );
716 break;
717 case MBQUAD:
718 norm = quad_norm( elem_corners[indices[0]], elem_corners[indices[1]], elem_corners[indices[2]],
719 elem_corners[indices[3]] );
720 break;
721 default:
722 assert( false );
723 continue;
724 }
725
726 dot = dot_abs( norm, dims );
727
728 // for each element vertex
729 not_less[0] = not_greater[0] = num_corner;
730 for( i = 0; i < num_corner; ++i )
731 {
732 tmp = norm % elem_corners[i];
733 not_less[0] -= ( tmp < -dot );
734 not_greater[0] -= ( tmp > dot );
735 }
736
737 if( not_less[0] * not_greater[0] == 0 ) return false;
738 }
739
740 // Overlap on all tested axes.
741 return true;
742 }
743
744 bool box_hex_overlap( const CartVect* elem_corners, const CartVect& center, const CartVect& dims )
745 {
746 // Do Separating Axis Theorem:
747 // If the element and the box overlap, then the 1D projections
748 // onto at least one of the axes in the following three sets
749 // must overlap (assuming convex polyhedral element).
750 // 1) The normals of the faces of the box (the principal axes)
751 // 2) The crossproduct of each element edge with each box edge
752 // (crossproduct of each edge with each principal axis)
753 // 3) The normals of the faces of the element
754
755 unsigned i, e, f; // loop counters
756 double dot, cross[2], tmp;
757 CartVect norm;
758 const CartVect corners[8] = { elem_corners[0] - center, elem_corners[1] - center, elem_corners[2] - center,
759 elem_corners[3] - center, elem_corners[4] - center, elem_corners[5] - center,
760 elem_corners[6] - center, elem_corners[7] - center };
761
762 // test box face normals (principal axes)
763 int not_less[3] = { 8, 8, 8 };
764 int not_greater[3] = { 8, 8, 8 };
765 int not_inside;
766 for( i = 0; i < 8; ++i )
767 { // for each element corner
768 not_inside = 3;
769
770 if( corners[i][0] < -dims[0] )
771 --not_less[0];
772 else if( corners[i][0] > dims[0] )
773 --not_greater[0];
774 else
775 --not_inside;
776
777 if( corners[i][1] < -dims[1] )
778 --not_less[1];
779 else if( corners[i][1] > dims[1] )
780 --not_greater[1];
781 else
782 --not_inside;
783
784 if( corners[i][2] < -dims[2] )
785 --not_less[2];
786 else if( corners[i][2] > dims[2] )
787 --not_greater[2];
788 else
789 --not_inside;
790
791 if( !not_inside ) return true;
792 }
793 // If all points less than min_x of box, then
794 // not_less[0] == 0, and therefore
795 // the following product is zero.
796 if( not_greater[0] * not_greater[1] * not_greater[2] * not_less[0] * not_less[1] * not_less[2] == 0 )
797 return false;
798
799 // Test edge-edge crossproducts
800 const unsigned edges[12][2] = { { 0, 1 }, { 0, 4 }, { 0, 3 }, { 2, 3 }, { 2, 1 }, { 2, 6 },
801 { 5, 6 }, { 5, 1 }, { 5, 4 }, { 7, 4 }, { 7, 3 }, { 7, 6 } };
802
803 // Edge directions for box are principal axis, so
804 // for each element edge, check along the cross-product
805 // of that edge with each of the tree principal axes.
806 for( e = 0; e < 12; ++e )
807 { // for each element edge
808 // get which element vertices bound the edge
809 const CartVect& v0 = corners[edges[e][0]];
810 const CartVect& v1 = corners[edges[e][1]];
811
812 // X-Axis
813
814 // calculate crossproduct: axis x (v1 - v0),
815 // where v1 and v0 are edge vertices.
816 cross[0] = v0[2] - v1[2];
817 cross[1] = v1[1] - v0[1];
818 // skip if parallel
819 if( ( cross[0] * cross[0] + cross[1] * cross[1] ) >= std::numeric_limits< double >::epsilon() )
820 {
821 dot = fabs( cross[0] * dims[1] ) + fabs( cross[1] * dims[2] );
822 not_less[0] = not_greater[0] = 7;
823 for( i = ( edges[e][0] + 1 ) % 8; i != edges[e][0]; i = ( i + 1 ) % 8 )
824 { // for each element corner
825 tmp = cross[0] * corners[i][1] + cross[1] * corners[i][2];
826 not_less[0] -= ( tmp < -dot );
827 not_greater[0] -= ( tmp > dot );
828 }
829
830 if( not_less[0] * not_greater[0] == 0 ) return false;
831 }
832
833 // Y-Axis
834
835 // calculate crossproduct: axis x (v1 - v0),
836 // where v1 and v0 are edge vertices.
837 cross[0] = v0[0] - v1[0];
838 cross[1] = v1[2] - v0[2];
839 // skip if parallel
840 if( ( cross[0] * cross[0] + cross[1] * cross[1] ) >= std::numeric_limits< double >::epsilon() )
841 {
842 dot = fabs( cross[0] * dims[2] ) + fabs( cross[1] * dims[0] );
843 not_less[0] = not_greater[0] = 7;
844 for( i = ( edges[e][0] + 1 ) % 8; i != edges[e][0]; i = ( i + 1 ) % 8 )
845 { // for each element corner
846 tmp = cross[0] * corners[i][2] + cross[1] * corners[i][0];
847 not_less[0] -= ( tmp < -dot );
848 not_greater[0] -= ( tmp > dot );
849 }
850
851 if( not_less[0] * not_greater[0] == 0 ) return false;
852 }
853
854 // Z-Axis
855
856 // calculate crossproduct: axis x (v1 - v0),
857 // where v1 and v0 are edge vertices.
858 cross[0] = v0[1] - v1[1];
859 cross[1] = v1[0] - v0[0];
860 // skip if parallel
861 if( ( cross[0] * cross[0] + cross[1] * cross[1] ) >= std::numeric_limits< double >::epsilon() )
862 {
863 dot = fabs( cross[0] * dims[0] ) + fabs( cross[1] * dims[1] );
864 not_less[0] = not_greater[0] = 7;
865 for( i = ( edges[e][0] + 1 ) % 8; i != edges[e][0]; i = ( i + 1 ) % 8 )
866 { // for each element corner
867 tmp = cross[0] * corners[i][0] + cross[1] * corners[i][1];
868 not_less[0] -= ( tmp < -dot );
869 not_greater[0] -= ( tmp > dot );
870 }
871
872 if( not_less[0] * not_greater[0] == 0 ) return false;
873 }
874 }
875
876 // test element face normals
877 const unsigned faces[6][4] = { { 0, 1, 2, 3 }, { 0, 1, 5, 4 }, { 1, 2, 6, 5 },
878 { 2, 6, 7, 3 }, { 3, 7, 4, 0 }, { 7, 4, 5, 6 } };
879 for( f = 0; f < 6; ++f )
880 {
881 norm = quad_norm( corners[faces[f][0]], corners[faces[f][1]], corners[faces[f][2]], corners[faces[f][3]] );
882
883 dot = dot_abs( norm, dims );
884
885 // for each element vertex
886 not_less[0] = not_greater[0] = 8;
887 for( i = 0; i < 8; ++i )
888 {
889 tmp = norm % corners[i];
890 not_less[0] -= ( tmp < -dot );
891 not_greater[0] -= ( tmp > dot );
892 }
893
894 if( not_less[0] * not_greater[0] == 0 ) return false;
895 }
896
897 // Overlap on all tested axes.
898 return true;
899 }
900
901 static inline bool box_tet_overlap_edge( const CartVect& dims,
902 const CartVect& edge,
903 const CartVect& ve,
904 const CartVect& v1,
905 const CartVect& v2 )
906 {
907 double dot, dot1, dot2, dot3, min, max;
908
909 // edge x X
910 if( fabs( edge[1] * edge[2] ) > std::numeric_limits< double >::epsilon() )
911 {
912 dot = fabs( edge[2] ) * dims[1] + fabs( edge[1] ) * dims[2];
913 dot1 = edge[2] * ve[1] - edge[1] * ve[2];
914 dot2 = edge[2] * v1[1] - edge[1] * v1[2];
915 dot3 = edge[2] * v2[1] - edge[1] * v2[2];
916 min_max_3( dot1, dot2, dot3, min, max );
917 if( max < -dot || min > dot ) return false;
918 }
919
920 // edge x Y
921 if( fabs( edge[1] * edge[2] ) > std::numeric_limits< double >::epsilon() )
922 {
923 dot = fabs( edge[2] ) * dims[0] + fabs( edge[0] ) * dims[2];
924 dot1 = -edge[2] * ve[0] + edge[0] * ve[2];
925 dot2 = -edge[2] * v1[0] + edge[0] * v1[2];
926 dot3 = -edge[2] * v2[0] + edge[0] * v2[2];
927 min_max_3( dot1, dot2, dot3, min, max );
928 if( max < -dot || min > dot ) return false;
929 }
930
931 // edge x Z
932 if( fabs( edge[1] * edge[2] ) > std::numeric_limits< double >::epsilon() )
933 {
934 dot = fabs( edge[1] ) * dims[0] + fabs( edge[0] ) * dims[1];
935 dot1 = edge[1] * ve[0] - edge[0] * ve[1];
936 dot2 = edge[1] * v1[0] - edge[0] * v1[1];
937 dot3 = edge[1] * v2[0] - edge[0] * v2[1];
938 min_max_3( dot1, dot2, dot3, min, max );
939 if( max < -dot || min > dot ) return false;
940 }
941
942 return true;
943 }
944
945 bool box_tet_overlap( const CartVect* corners_in, const CartVect& center, const CartVect& dims )
946 {
947 // Do Separating Axis Theorem:
948 // If the element and the box overlap, then the 1D projections
949 // onto at least one of the axes in the following three sets
950 // must overlap (assuming convex polyhedral element).
951 // 1) The normals of the faces of the box (the principal axes)
952 // 2) The crossproduct of each element edge with each box edge
953 // (crossproduct of each edge with each principal axis)
954 // 3) The normals of the faces of the element
955
956 // Translate problem such that box center is at origin.
957 const CartVect corners[4] = { corners_in[0] - center, corners_in[1] - center, corners_in[2] - center,
958 corners_in[3] - center };
959
960 // 0) Check if any vertex is within the box
961 if( fabs( corners[0][0] ) <= dims[0] && fabs( corners[0][1] ) <= dims[1] && fabs( corners[0][2] ) <= dims[2] )
962 return true;
963 if( fabs( corners[1][0] ) <= dims[0] && fabs( corners[1][1] ) <= dims[1] && fabs( corners[1][2] ) <= dims[2] )
964 return true;
965 if( fabs( corners[2][0] ) <= dims[0] && fabs( corners[2][1] ) <= dims[1] && fabs( corners[2][2] ) <= dims[2] )
966 return true;
967 if( fabs( corners[3][0] ) <= dims[0] && fabs( corners[3][1] ) <= dims[1] && fabs( corners[3][2] ) <= dims[2] )
968 return true;
969
970 // 1) Check for overlap on each principal axis (box face normal)
971 // X
972 if( corners[0][0] < -dims[0] && corners[1][0] < -dims[0] && corners[2][0] < -dims[0] &&
973 corners[3][0] < -dims[0] )
974 return false;
975 if( corners[0][0] > dims[0] && corners[1][0] > dims[0] && corners[2][0] > dims[0] && corners[3][0] > dims[0] )
976 return false;
977 // Y
978 if( corners[0][1] < -dims[1] && corners[1][1] < -dims[1] && corners[2][1] < -dims[1] &&
979 corners[3][1] < -dims[1] )
980 return false;
981 if( corners[0][1] > dims[1] && corners[1][1] > dims[1] && corners[2][1] > dims[1] && corners[3][1] > dims[1] )
982 return false;
983 // Z
984 if( corners[0][2] < -dims[2] && corners[1][2] < -dims[2] && corners[2][2] < -dims[2] &&
985 corners[3][2] < -dims[2] )
986 return false;
987 if( corners[0][2] > dims[2] && corners[1][2] > dims[2] && corners[2][2] > dims[2] && corners[3][2] > dims[2] )
988 return false;
989
990 // 3) test element face normals
991 CartVect norm;
992 double dot, dot1, dot2;
993
994 const CartVect v01 = corners[1] - corners[0];
995 const CartVect v02 = corners[2] - corners[0];
996 norm = v01 * v02;
997 dot = dot_abs( norm, dims );
998 dot1 = norm % corners[0];
999 dot2 = norm % corners[3];
1000 if( dot1 > dot2 ) std::swap( dot1, dot2 );
1001 if( dot2 < -dot || dot1 > dot ) return false;
1002
1003 const CartVect v03 = corners[3] - corners[0];
1004 norm = v03 * v01;
1005 dot = dot_abs( norm, dims );
1006 dot1 = norm % corners[0];
1007 dot2 = norm % corners[2];
1008 if( dot1 > dot2 ) std::swap( dot1, dot2 );
1009 if( dot2 < -dot || dot1 > dot ) return false;
1010
1011 norm = v02 * v03;
1012 dot = dot_abs( norm, dims );
1013 dot1 = norm % corners[0];
1014 dot2 = norm % corners[1];
1015 if( dot1 > dot2 ) std::swap( dot1, dot2 );
1016 if( dot2 < -dot || dot1 > dot ) return false;
1017
1018 const CartVect v12 = corners[2] - corners[1];
1019 const CartVect v13 = corners[3] - corners[1];
1020 norm = v13 * v12;
1021 dot = dot_abs( norm, dims );
1022 dot1 = norm % corners[0];
1023 dot2 = norm % corners[1];
1024 if( dot1 > dot2 ) std::swap( dot1, dot2 );
1025 if( dot2 < -dot || dot1 > dot ) return false;
1026
1027 // 2) test edge-edge cross products
1028
1029 const CartVect v23 = corners[3] - corners[2];
1030 return box_tet_overlap_edge( dims, v01, corners[0], corners[2], corners[3] ) &&
1031 box_tet_overlap_edge( dims, v02, corners[0], corners[1], corners[3] ) &&
1032 box_tet_overlap_edge( dims, v03, corners[0], corners[1], corners[2] ) &&
1033 box_tet_overlap_edge( dims, v12, corners[1], corners[0], corners[3] ) &&
1034 box_tet_overlap_edge( dims, v13, corners[1], corners[0], corners[2] ) &&
1035 box_tet_overlap_edge( dims, v23, corners[2], corners[0], corners[1] );
1036 }
1037
1038 // from:
1039 // http://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf#search=%22closest%20point%20on%20triangle%22
1040 /* t
1041 * \(2)^
1042 * \ |
1043 * \ |
1044 * \|
1045 * \
1046 * |\
1047 * | \
1048 * | \ (1)
1049 * (3) tv \
1050 * | \
1051 * | (0) \
1052 * | \
1053 *-------+---sv--\----> s
1054 * | \ (6)
1055 * (4) | (5) \
1056 */
1057 // Worst case is either 61 flops and 5 compares or 53 flops and 6 compares,
1058 // depending on relative costs. For all paths that do not return one of the
1059 // corner vertices, exactly one of the flops is a divide.
1060 void closest_location_on_tri( const CartVect& location, const CartVect* vertices, CartVect& closest_out )
1061 { // ops comparisons
1062 const CartVect sv( vertices[1] - vertices[0] ); // +3 = 3
1063 const CartVect tv( vertices[2] - vertices[0] ); // +3 = 6
1064 const CartVect pv( vertices[0] - location ); // +3 = 9
1065 const double ss = sv % sv; // +5 = 14
1066 const double st = sv % tv; // +5 = 19
1067 const double tt = tv % tv; // +5 = 24
1068 const double sp = sv % pv; // +5 = 29
1069 const double tp = tv % pv; // +5 = 34
1070 const double det = ss * tt - st * st; // +3 = 37
1071 double s = st * tp - tt * sp; // +3 = 40
1072 double t = st * sp - ss * tp; // +3 = 43
1073 if( s + t < det )
1074 { // +1 = 44, +1 = 1
1075 if( s < 0 )
1076 { // +1 = 2
1077 if( t < 0 )
1078 { // +1 = 3
1079 // region 4
1080 if( sp < 0 )
1081 { // +1 = 4
1082 if( -sp > ss ) // +1 = 5
1083 closest_out = vertices[1]; // 44 5
1084 else
1085 closest_out = vertices[0] - ( sp / ss ) * sv; // +7 = 51, 5
1086 }
1087 else if( tp < 0 )
1088 { // +1 = 5
1089 if( -tp > tt ) // +1 = 6
1090 closest_out = vertices[2]; // 44, 6
1091 else
1092 closest_out = vertices[0] - ( tp / tt ) * tv; // +7 = 51, 6
1093 }
1094 else
1095 {
1096 closest_out = vertices[0]; // 44, 5
1097 }
1098 }
1099 else
1100 {
1101 // region 3
1102 if( tp >= 0 ) // +1 = 4
1103 closest_out = vertices[0]; // 44, 4
1104 else if( -tp >= tt ) // +1 = 5
1105 closest_out = vertices[2]; // 44, 5
1106 else
1107 closest_out = vertices[0] - ( tp / tt ) * tv; // +7 = 51, 5
1108 }
1109 }
1110 else if( t < 0 )
1111 { // +1 = 3
1112 // region 5;
1113 if( sp >= 0.0 ) // +1 = 4
1114 closest_out = vertices[0]; // 44, 4
1115 else if( -sp >= ss ) // +1 = 5
1116 closest_out = vertices[1]; // 44 5
1117 else
1118 closest_out = vertices[0] - ( sp / ss ) * sv; // +7 = 51, 5
1119 }
1120 else
1121 {
1122 // region 0
1123 const double inv_det = 1.0 / det; // +1 = 45
1124 s *= inv_det; // +1 = 46
1125 t *= inv_det; // +1 = 47
1126 closest_out = vertices[0] + s * sv + t * tv; //+12 = 59, 3
1127 }
1128 }
1129 else
1130 {
1131 if( s < 0 )
1132 { // +1 = 2
1133 // region 2
1134 s = st + sp; // +1 = 45
1135 t = tt + tp; // +1 = 46
1136 if( t > s )
1137 { // +1 = 3
1138 const double num = t - s; // +1 = 47
1139 const double den = ss - 2 * st + tt; // +3 = 50
1140 if( num > den ) // +1 = 4
1141 closest_out = vertices[1]; // 50, 4
1142 else
1143 {
1144 s = num / den; // +1 = 51
1145 t = 1 - s; // +1 = 52
1146 closest_out = s * vertices[1] + t * vertices[2]; // +9 = 61, 4
1147 }
1148 }
1149 else if( t <= 0 ) // +1 = 4
1150 closest_out = vertices[2]; // 46, 4
1151 else if( tp >= 0 ) // +1 = 5
1152 closest_out = vertices[0]; // 46, 5
1153 else
1154 closest_out = vertices[0] - ( tp / tt ) * tv; // +7 = 53, 5
1155 }
1156 else if( t < 0 )
1157 { // +1 = 3
1158 // region 6
1159 t = st + tp; // +1 = 45
1160 s = ss + sp; // +1 = 46
1161 if( s > t )
1162 { // +1 = 4
1163 const double num = t - s; // +1 = 47
1164 const double den = tt - 2 * st + ss; // +3 = 50
1165 if( num > den ) // +1 = 5
1166 closest_out = vertices[2]; // 50, 5
1167 else
1168 {
1169 t = num / den; // +1 = 51
1170 s = 1 - t; // +1 = 52
1171 closest_out = s * vertices[1] + t * vertices[2]; // +9 = 61, 5
1172 }
1173 }
1174 else if( s <= 0 ) // +1 = 5
1175 closest_out = vertices[1]; // 46, 5
1176 else if( sp >= 0 ) // +1 = 6
1177 closest_out = vertices[0]; // 46, 6
1178 else
1179 closest_out = vertices[0] - ( sp / ss ) * sv; // +7 = 53, 6
1180 }
1181 else
1182 {
1183 // region 1
1184 const double num = tt + tp - st - sp; // +3 = 47
1185 if( num <= 0 )
1186 { // +1 = 4
1187 closest_out = vertices[2]; // 47, 4
1188 }
1189 else
1190 {
1191 const double den = ss - 2 * st + tt; // +3 = 50
1192 if( num >= den ) // +1 = 5
1193 closest_out = vertices[1]; // 50, 5
1194 else
1195 {
1196 s = num / den; // +1 = 51
1197 t = 1 - s; // +1 = 52
1198 closest_out = s * vertices[1] + t * vertices[2]; // +9 = 61, 5
1199 }
1200 }
1201 }
1202 }
1203 }
1204
1205 void closest_location_on_tri( const CartVect& location,
1206 const CartVect* vertices,
1207 double tolerance,
1208 CartVect& closest_out,
1209 int& closest_topo )
1210 {
1211 const double tsqr = tolerance * tolerance;
1212 int i;
1213 CartVect pv[3], ev, ep;
1214 double t;
1215
1216 closest_location_on_tri( location, vertices, closest_out );
1217
1218 for( i = 0; i < 3; ++i )
1219 {
1220 pv[i] = vertices[i] - closest_out;
1221 if( ( pv[i] % pv[i] ) <= tsqr )
1222 {
1223 closest_topo = i;
1224 return;
1225 }
1226 }
1227
1228 for( i = 0; i < 3; ++i )
1229 {
1230 ev = vertices[( i + 1 ) % 3] - vertices[i];
1231 t = ( ev % pv[i] ) / ( ev % ev );
1232 ep = closest_out - ( vertices[i] + t * ev );
1233 if( ( ep % ep ) <= tsqr )
1234 {
1235 closest_topo = i + 3;
1236 return;
1237 }
1238 }
1239
1240 closest_topo = 6;
1241 }
1242
1243 // We assume polygon is *convex*, but *not* planar.
1244 void closest_location_on_polygon( const CartVect& location,
1245 const CartVect* vertices,
1246 int num_vertices,
1247 CartVect& closest_out )
1248 {
1249 const int n = num_vertices;
1250 CartVect d, p, v;
1251 double shortest_sqr, dist_sqr, t_closest, t;
1252 int i, e;
1253
1254 // Find closest edge of polygon.
1255 e = n - 1;
1256 v = vertices[0] - vertices[e];
1257 t_closest = ( v % ( location - vertices[e] ) ) / ( v % v );
1258 if( t_closest < 0.0 )
1259 d = location - vertices[e];
1260 else if( t_closest > 1.0 )
1261 d = location - vertices[0];
1262 else
1263 d = location - vertices[e] - t_closest * v;
1264 shortest_sqr = d % d;
1265 for( i = 0; i < n - 1; ++i )
1266 {
1267 v = vertices[i + 1] - vertices[i];
1268 t = ( v % ( location - vertices[i] ) ) / ( v % v );
1269 if( t < 0.0 )
1270 d = location - vertices[i];
1271 else if( t > 1.0 )
1272 d = location - vertices[i + 1];
1273 else
1274 d = location - vertices[i] - t * v;
1275 dist_sqr = d % d;
1276 if( dist_sqr < shortest_sqr )
1277 {
1278 e = i;
1279 shortest_sqr = dist_sqr;
1280 t_closest = t;
1281 }
1282 }
1283
1284 // If we are beyond the bounds of the edge, then
1285 // the point is outside and closest to a vertex
1286 if( t_closest <= 0.0 )
1287 {
1288 closest_out = vertices[e];
1289 return;
1290 }
1291 else if( t_closest >= 1.0 )
1292 {
1293 closest_out = vertices[( e + 1 ) % n];
1294 return;
1295 }
1296
1297 // Now check which side of the edge we are one
1298 const CartVect v0 = vertices[e] - vertices[( e + n - 1 ) % n];
1299 const CartVect v1 = vertices[( e + 1 ) % n] - vertices[e];
1300 const CartVect v2 = vertices[( e + 2 ) % n] - vertices[( e + 1 ) % n];
1301 const CartVect norm = ( 1.0 - t_closest ) * ( v0 * v1 ) + t_closest * ( v1 * v2 );
1302 // if on outside of edge, result is closest point on edge
1303 if( ( norm % ( ( vertices[e] - location ) * v1 ) ) <= 0.0 )
1304 {
1305 closest_out = vertices[e] + t_closest * v1;
1306 return;
1307 }
1308
1309 // Inside. Project to plane defined by point and normal at
1310 // closest edge
1311 const double D = -( norm % ( vertices[e] + t_closest * v1 ) );
1312 closest_out = ( location - ( norm % location + D ) * norm ) / ( norm % norm );
1313 }
1314
1315 void closest_location_on_box( const CartVect& min, const CartVect& max, const CartVect& point, CartVect& closest )
1316 {
1317 closest[0] = point[0] < min[0] ? min[0] : point[0] > max[0] ? max[0] : point[0];
1318 closest[1] = point[1] < min[1] ? min[1] : point[1] > max[1] ? max[1] : point[1];
1319 closest[2] = point[2] < min[2] ? min[2] : point[2] > max[2] ? max[2] : point[2];
1320 }
1321
1322 bool box_point_overlap( const CartVect& box_min_corner,
1323 const CartVect& box_max_corner,
1324 const CartVect& point,
1325 double tolerance )
1326 {
1327 CartVect closest;
1328 closest_location_on_box( box_min_corner, box_max_corner, point, closest );
1329 closest -= point;
1330 return closest % closest < tolerance * tolerance;
1331 }
1332
1333 bool boxes_overlap( const CartVect& box_min1,
1334 const CartVect& box_max1,
1335 const CartVect& box_min2,
1336 const CartVect& box_max2,
1337 double tolerance )
1338 {
1339
1340 for( int k = 0; k < 3; k++ )
1341 {
1342 double b1min = box_min1[k], b1max = box_max1[k];
1343 double b2min = box_min2[k], b2max = box_max2[k];
1344 if( b1min - tolerance > b2max ) return false;
1345 if( b2min - tolerance > b1max ) return false;
1346 }
1347 return true;
1348 }
1349
1350 // see if boxes formed by 2 lists of "CartVect"s overlap
1351 bool bounding_boxes_overlap( const CartVect* list1, int num1, const CartVect* list2, int num2, double tolerance )
1352 {
1353 assert( num1 >= 1 && num2 >= 1 );
1354 CartVect box_min1 = list1[0], box_max1 = list1[0];
1355 CartVect box_min2 = list2[0], box_max2 = list2[0];
1356 for( int i = 1; i < num1; i++ )
1357 {
1358 for( int k = 0; k < 3; k++ )
1359 {
1360 double val = list1[i][k];
1361 if( box_min1[k] > val ) box_min1[k] = val;
1362 if( box_max1[k] < val ) box_max1[k] = val;
1363 }
1364 }
1365 for( int i = 1; i < num2; i++ )
1366 {
1367 for( int k = 0; k < 3; k++ )
1368 {
1369 double val = list2[i][k];
1370 if( box_min2[k] > val ) box_min2[k] = val;
1371 if( box_max2[k] < val ) box_max2[k] = val;
1372 }
1373 }
1374
1375 return boxes_overlap( box_min1, box_max1, box_min2, box_max2, tolerance );
1376 }
1377
1378 // see if boxes formed by 2 lists of 2d coordinates overlap (num1>=3, num2>=3, do not check)
1379 bool bounding_boxes_overlap_2d( const double* list1, int num1, const double* list2, int num2, double tolerance )
1380 {
1381 /*
1382 * box1:
1383 * (bmax11, bmax12)
1384 * |-------|
1385 * | |
1386 * |-------|
1387 * (bmin11, bmin12)
1388
1389 * box2:
1390 * (bmax21, bmax22)
1391 * |----------|
1392 * | |
1393 * |----------|
1394 * (bmin21, bmin22)
1395 */
1396 double bmin11, bmax11, bmin12, bmax12;
1397 bmin11 = bmax11 = list1[0];
1398 bmin12 = bmax12 = list1[1];
1399
1400 double bmin21, bmax21, bmin22, bmax22;
1401 bmin21 = bmax21 = list2[0];
1402 bmin22 = bmax22 = list2[1];
1403
1404 for( int i = 1; i < num1; i++ )
1405 {
1406 if( bmin11 > list1[2 * i] ) bmin11 = list1[2 * i];
1407 if( bmax11 < list1[2 * i] ) bmax11 = list1[2 * i];
1408 if( bmin12 > list1[2 * i + 1] ) bmin12 = list1[2 * i + 1];
1409 if( bmax12 < list1[2 * i + 1] ) bmax12 = list1[2 * i + 1];
1410 }
1411 for( int i = 1; i < num2; i++ )
1412 {
1413 if( bmin21 > list2[2 * i] ) bmin21 = list2[2 * i];
1414 if( bmax21 < list2[2 * i] ) bmax21 = list2[2 * i];
1415 if( bmin22 > list2[2 * i + 1] ) bmin22 = list2[2 * i + 1];
1416 if( bmax22 < list2[2 * i + 1] ) bmax22 = list2[2 * i + 1];
1417 }
1418
1419 if( ( bmax11 < bmin21 + tolerance ) || ( bmax21 < bmin11 + tolerance ) ) return false;
1420
1421 if( ( bmax12 < bmin22 + tolerance ) || ( bmax22 < bmin12 + tolerance ) ) return false;
1422
1423 return true;
1424 }
1425
1426 /**\brief Class representing a 3-D mapping function (e.g. shape function for volume element) */
1427 class VolMap
1428 {
1429 public:
1430 /**\brief Return \f$\vec \xi\f$ corresponding to logical center of element */
1431 virtual CartVect center_xi() const = 0;
1432 /**\brief Evaluate mapping function (calculate \f$\vec x = F($\vec \xi)\f$ )*/
1433 virtual CartVect evaluate( const CartVect& xi ) const = 0;
1434 /**\brief Evaluate Jacobian of mapping function */
1435 virtual Matrix3 jacobian( const CartVect& xi ) const = 0;
1436 /**\brief Evaluate inverse of mapping function (calculate \f$\vec \xi = F^-1($\vec x)\f$ )*/
1437 bool solve_inverse( const CartVect& x, CartVect& xi, double tol ) const;
1438 };
1439
1440 bool VolMap::solve_inverse( const CartVect& x, CartVect& xi, double tol ) const
1441 {
1442 const double error_tol_sqr = tol * tol;
1443 double det;
1444 xi = center_xi();
1445 CartVect delta = evaluate( xi ) - x;
1446 Matrix3 J;
1447 while( delta % delta > error_tol_sqr )
1448 {
1449 J = jacobian( xi );
1450 det = J.determinant();
1451 if( det < std::numeric_limits< double >::epsilon() ) return false;
1452 xi -= J.inverse() * delta;
1453 delta = evaluate( xi ) - x;
1454 }
1455 return true;
1456 }
1457
1458 /**\brief Shape function for trilinear hexahedron */
1459 class LinearHexMap : public VolMap
1460 {
1461 public:
1462 LinearHexMap( const CartVect* corner_coords ) : corners( corner_coords ) {}
1463 virtual CartVect center_xi() const;
1464 virtual CartVect evaluate( const CartVect& xi ) const;
1465 virtual Matrix3 jacobian( const CartVect& xi ) const;
1466
1467 private:
1468 const CartVect* corners;
1469 static const double corner_xi[8][3];
1470 };
1471
1472 const double LinearHexMap::corner_xi[8][3] = { { -1, -1, -1 }, { 1, -1, -1 }, { 1, 1, -1 }, { -1, 1, -1 },
1473 { -1, -1, 1 }, { 1, -1, 1 }, { 1, 1, 1 }, { -1, 1, 1 } };
1474 CartVect LinearHexMap::center_xi() const
1475 {
1476 return CartVect( 0.0 );
1477 }
1478
1479 CartVect LinearHexMap::evaluate( const CartVect& xi ) const
1480 {
1481 CartVect x( 0.0 );
1482 for( unsigned i = 0; i < 8; ++i )
1483 {
1484 const double N_i =
1485 ( 1 + xi[0] * corner_xi[i][0] ) * ( 1 + xi[1] * corner_xi[i][1] ) * ( 1 + xi[2] * corner_xi[i][2] );
1486 x += N_i * corners[i];
1487 }
1488 x *= 0.125;
1489 return x;
1490 }
1491
1492 Matrix3 LinearHexMap::jacobian( const CartVect& xi ) const
1493 {
1494 Matrix3 J( 0.0 );
1495 for( unsigned i = 0; i < 8; ++i )
1496 {
1497 const double xi_p = 1 + xi[0] * corner_xi[i][0];
1498 const double eta_p = 1 + xi[1] * corner_xi[i][1];
1499 const double zeta_p = 1 + xi[2] * corner_xi[i][2];
1500 const double dNi_dxi = corner_xi[i][0] * eta_p * zeta_p;
1501 const double dNi_deta = corner_xi[i][1] * xi_p * zeta_p;
1502 const double dNi_dzeta = corner_xi[i][2] * xi_p * eta_p;
1503 J( 0, 0 ) += dNi_dxi * corners[i][0];
1504 J( 1, 0 ) += dNi_dxi * corners[i][1];
1505 J( 2, 0 ) += dNi_dxi * corners[i][2];
1506 J( 0, 1 ) += dNi_deta * corners[i][0];
1507 J( 1, 1 ) += dNi_deta * corners[i][1];
1508 J( 2, 1 ) += dNi_deta * corners[i][2];
1509 J( 0, 2 ) += dNi_dzeta * corners[i][0];
1510 J( 1, 2 ) += dNi_dzeta * corners[i][1];
1511 J( 2, 2 ) += dNi_dzeta * corners[i][2];
1512 }
1513 return J *= 0.125;
1514 }
1515
1516 bool nat_coords_trilinear_hex( const CartVect* corner_coords, const CartVect& x, CartVect& xi, double tol )
1517 {
1518 return LinearHexMap( corner_coords ).solve_inverse( x, xi, tol );
1519 }
1520
1521 bool point_in_trilinear_hex( const CartVect* hex, const CartVect& xyz, double etol )
1522 {
1523 CartVect xi;
1524 return nat_coords_trilinear_hex( hex, xyz, xi, etol ) && fabs( xi[0] ) - 1 < etol && fabs( xi[1] ) - 1 < etol &&
1525 fabs( xi[2] ) - 1 < etol;
1526 }
1527
1528 } // namespace GeomUtil
1529
1530 } // namespace moab
1.9.1.