Mesh Oriented datABase  (version 5.5.1)
An array-based unstructured mesh library
GeomUtil.cpp
Go to the documentation of this file.
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 ([email protected])
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