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