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 : : /*
17 : : * The algorithms for the calculation of the oriented box from a
18 : : * set of points or a set of cells was copied from the implementation
19 : : " in the "Visualization Toolkit". J.K. - 2006-07-19
20 : : *
21 : : * Program: Visualization Toolkit
22 : : * Module: $RCSfile$
23 : : *
24 : : * Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
25 : : * All rights reserved.
26 : : * See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
27 : : */
28 : :
29 : : /**\file OrientedBox.cpp
30 : : *\author Jason Kraftcheck ([email protected])
31 : : *\date 2006-07-18
32 : : */
33 : :
34 : : #include "moab/Interface.hpp"
35 : : #include "moab/CN.hpp"
36 : : #include "moab/OrientedBox.hpp"
37 : : #include "moab/Range.hpp"
38 : : #include <ostream>
39 : : #include <assert.h>
40 : : #include <limits>
41 : :
42 : : namespace moab
43 : : {
44 : :
45 : 0 : std::ostream& operator<<( std::ostream& s, const OrientedBox& b )
46 : : {
47 [ # # ][ # # ]: 0 : return s << b.center << " + " << b.axes.col( 0 )
[ # # ][ # # ]
48 : : #if MB_ORIENTED_BOX_UNIT_VECTORS
49 [ # # ][ # # ]: 0 : << ":" << b.length[0]
[ # # ]
50 : : #endif
51 [ # # ][ # # ]: 0 : << " x " << b.axes.col( 1 )
[ # # ]
52 : : #if MB_ORIENTED_BOX_UNIT_VECTORS
53 [ # # ][ # # ]: 0 : << ":" << b.length[1]
[ # # ]
54 : : #endif
55 [ # # ][ # # ]: 0 : << " x " << b.axes.col( 2 )
56 : : #if MB_ORIENTED_BOX_UNIT_VECTORS
57 [ # # ][ # # ]: 0 : << ":" << b.length[2]
58 : : #endif
59 : : ;
60 : : }
61 : :
62 : : /**\brief Find closest point on line
63 : : *
64 : : * Find the point on the line for which a line trough the
65 : : * input point \a p and the result position is orthogonal to
66 : : * the input line.
67 : : * \param p The point for which to find the perpendicular
68 : : * \param b A point on the line
69 : : * \param m The direction of the line
70 : : * \return The location on the line specified as 't' in the
71 : : * formula t * m + b
72 : : */
73 : 1954305 : static double point_perp( const CartVect& p, // closest to this point
74 : : const CartVect& b, // point on line
75 : : const CartVect& m ) // line direction
76 : : {
77 : : #if MB_ORIENTED_BOX_UNIT_VECTORS
78 [ + - ]: 1954305 : double t = ( m % ( p - b ) );
79 : : #else
80 : : double t = ( m % ( p - b ) ) / ( m % m );
81 : : #endif
82 [ + - ]: 1954305 : return Util::is_finite( t ) ? t : 0.0;
83 : : }
84 : :
85 : 5 : void OrientedBox::order_axes_by_length( double ax1_len, double ax2_len, double ax3_len )
86 : : {
87 : :
88 [ + - ]: 5 : CartVect len( ax1_len, ax2_len, ax3_len );
89 : :
90 [ + - ][ + - ]: 5 : if( len[2] < len[1] )
[ + + ]
91 : : {
92 [ + - ][ + - ]: 1 : if( len[2] < len[0] )
[ + - ]
93 : : {
94 [ + - ][ + - ]: 1 : std::swap( len[0], len[2] );
95 [ + - ]: 1 : axes.swapcol( 0, 2 );
96 : : }
97 : : }
98 [ + - ][ + - ]: 4 : else if( len[1] < len[0] )
[ - + ]
99 : : {
100 [ # # ][ # # ]: 0 : std::swap( len[0], len[1] );
101 [ # # ]: 0 : axes.swapcol( 0, 1 );
102 : : }
103 [ + - ][ + - ]: 5 : if( len[1] > len[2] )
[ + + ]
104 : : {
105 [ + - ][ + - ]: 1 : std::swap( len[1], len[2] );
106 [ + - ]: 1 : axes.swapcol( 1, 2 );
107 : : }
108 : :
109 : : #if MB_ORIENTED_BOX_UNIT_VECTORS
110 : 5 : length = len;
111 [ + - ][ + - ]: 5 : if( len[0] > 0.0 ) axes.colscale( 0, 1.0 / len[0] );
[ + - ][ + - ]
112 [ + - ][ + - ]: 5 : if( len[1] > 0.0 ) axes.colscale( 1, 1.0 / len[1] );
[ + - ][ + - ]
113 [ + - ][ + - ]: 5 : if( len[2] > 0.0 ) axes.colscale( 2, 1.0 / len[2] );
[ + - ][ + - ]
114 : : #endif
115 : :
116 : : #if MB_ORIENTED_BOX_OUTER_RADIUS
117 [ + - ]: 5 : radius = len.length();
118 : : #endif
119 : 5 : }
120 : :
121 : 2 : OrientedBox::OrientedBox( const CartVect axes_in[3], const CartVect& mid ) : center( mid )
122 : : {
123 : :
124 [ + - ]: 1 : axes = Matrix3( axes_in[0], axes_in[1], axes_in[2], false );
125 : :
126 : 1 : order_axes_by_length( axes_in[0].length(), axes_in[1].length(), axes_in[2].length() );
127 : 1 : }
128 : :
129 : 8 : OrientedBox::OrientedBox( const Matrix3& axes_mat, const CartVect& mid ) : center( mid ), axes( axes_mat )
130 : : {
131 [ + - ][ + - ]: 4 : order_axes_by_length( axes.col( 0 ).length(), axes.col( 1 ).length(), axes.col( 2 ).length() );
[ + - ][ + - ]
[ + - ][ + - ]
132 : 4 : }
133 : :
134 : 203 : ErrorCode OrientedBox::tag_handle( Tag& handle_out, Interface* instance, const char* name )
135 : : {
136 : : // We're going to assume this when mapping the OrientedBox
137 : : // to tag data, so assert it.
138 : : #if MB_ORIENTED_BOX_OUTER_RADIUS
139 : 203 : const int rad_size = 1;
140 : : #else
141 : : const int rad_size = 0;
142 : : #endif
143 : : #if MB_ORIENTED_BOX_UNIT_VECTORS
144 : 203 : const int SIZE = rad_size + 15;
145 : : #else
146 : : const int SIZE = rad_size + 12;
147 : : #endif
148 [ - + ]: 203 : assert( sizeof( OrientedBox ) == SIZE * sizeof( double ) );
149 : :
150 : 203 : return instance->tag_get_handle( name, SIZE, MB_TYPE_DOUBLE, handle_out, MB_TAG_DENSE | MB_TAG_CREAT );
151 : : }
152 : :
153 : : /**\brief Common code for box calculation
154 : : *
155 : : * Given the orientation of the box and an approximate center,
156 : : * calculate the exact center and extents of the box.
157 : : *
158 : : *\param result.center As input, the approximate center of the box.
159 : : * As output, the exact center of the box.
160 : : *\param result.axes As input, directions of principal axes corresponding
161 : : * to the orientation of the box. Axes are assumed to
162 : : * be unit-length on input. Output will include extents
163 : : * of box.
164 : : *\param points The set of points the box should contain.
165 : : */
166 : 22702 : static ErrorCode box_from_axes( OrientedBox& result, Interface* instance, const Range& points )
167 : : {
168 : : ErrorCode rval;
169 : :
170 : : // project points onto axes to get box extents
171 [ + - ][ + - ]: 22702 : CartVect min( std::numeric_limits< double >::max() ), max( -std::numeric_limits< double >::max() );
172 [ + - ][ + - ]: 674137 : for( Range::iterator i = points.begin(); i != points.end(); ++i )
[ + - ][ + - ]
[ + + ]
173 : : {
174 [ + - ]: 651435 : CartVect coords;
175 [ + - ][ + - ]: 651435 : rval = instance->get_coords( &*i, 1, coords.array() );MB_CHK_ERR( rval );
[ + - ][ - + ]
[ # # ][ # # ]
176 : :
177 [ + + ]: 2605740 : for( int d = 0; d < 3; ++d )
178 : : {
179 [ + - ][ + - ]: 1954305 : const double t = point_perp( coords, result.center, result.axes.col( d ) );
180 [ + - ][ + + ]: 1954305 : if( t < min[d] ) min[d] = t;
[ + - ]
181 [ + - ][ + + ]: 1954305 : if( t > max[d] ) max[d] = t;
[ + - ]
182 : : }
183 : : }
184 : :
185 : : // We now have a box defined by three orthogonal line segments
186 : : // that intersect at the center of the box. Each line segment
187 : : // is defined as result.center + t * result.axes[i], where the
188 : : // range of t is [min[i], max[i]].
189 : :
190 : : // Calculate new center
191 [ + - ][ + - ]: 22702 : const CartVect mid = 0.5 * ( min + max );
192 [ + - ][ + - ]: 22702 : result.center += mid[0] * result.axes.col( 0 ) + mid[1] * result.axes.col( 1 ) + mid[2] * result.axes.col( 2 );
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
193 : :
194 : : // reorder axes by length
195 [ + - ][ + - ]: 22702 : CartVect range = 0.5 * ( max - min );
196 [ + - ][ + - ]: 22702 : if( range[2] < range[1] )
[ + + ]
197 : : {
198 [ + - ][ + - ]: 965 : if( range[2] < range[0] )
[ + + ]
199 : : {
200 [ + - ][ + - ]: 35 : std::swap( range[0], range[2] );
201 [ + - ]: 35 : result.axes.swapcol( 0, 2 );
202 : : }
203 : : }
204 [ + - ][ + - ]: 21737 : else if( range[1] < range[0] )
[ + + ]
205 : : {
206 [ + - ][ + - ]: 80 : std::swap( range[0], range[1] );
207 [ + - ]: 80 : result.axes.swapcol( 0, 1 );
208 : : }
209 [ + - ][ + - ]: 22702 : if( range[1] > range[2] )
[ + + ]
210 : : {
211 [ + - ][ + - ]: 930 : std::swap( range[1], range[2] );
212 [ + - ]: 930 : result.axes.swapcol( 1, 2 );
213 : : }
214 : :
215 : : // scale axis to encompass all points, divide in half
216 : : #if MB_ORIENTED_BOX_UNIT_VECTORS
217 : 22702 : result.length = range;
218 : : #else
219 : : result.axes.colscale( 0, range[0] );
220 : : result.axes.colscale( 1, range[1] );
221 : : result.axes.colscale( 2, range[2] );
222 : : #endif
223 : :
224 : : #if MB_ORIENTED_BOX_OUTER_RADIUS
225 [ + - ]: 22702 : result.radius = range.length();
226 : : #endif
227 : :
228 : 22702 : return MB_SUCCESS;
229 : : }
230 : :
231 : 1 : ErrorCode OrientedBox::compute_from_vertices( OrientedBox& result, Interface* instance, const Range& vertices )
232 : : {
233 [ + - ]: 1 : const Range::iterator begin = vertices.lower_bound( MBVERTEX );
234 [ + - ]: 1 : const Range::iterator end = vertices.upper_bound( MBVERTEX );
235 : 1 : size_t count = 0;
236 : :
237 : : // compute mean
238 [ + - ]: 1 : CartVect v;
239 [ + - ]: 1 : result.center = CartVect( 0, 0, 0 );
240 [ + - ][ + - ]: 9 : for( Range::iterator i = begin; i != end; ++i )
[ + + ]
241 : : {
242 [ + - ][ + - ]: 8 : ErrorCode rval = instance->get_coords( &*i, 1, v.array() );
[ + - ]
243 [ - + ]: 8 : if( MB_SUCCESS != rval ) return rval;
244 [ + - ]: 8 : result.center += v;
245 : 8 : ++count;
246 : : }
247 [ + - ]: 1 : result.center /= count;
248 : :
249 : : // compute covariance matrix
250 [ + - ]: 1 : Matrix3 a( 0.0 );
251 [ + - ][ + - ]: 9 : for( Range::iterator i = begin; i != end; ++i )
[ + + ]
252 : : {
253 [ + - ][ + - ]: 8 : ErrorCode rval = instance->get_coords( &*i, 1, v.array() );
[ + - ]
254 [ - + ]: 8 : if( MB_SUCCESS != rval ) return rval;
255 : :
256 [ + - ]: 8 : v -= result.center;
257 [ + - ][ + - ]: 8 : a += outer_product( v, v );
258 : : }
259 [ + - ]: 1 : a /= count;
260 : :
261 : : // Get axes (Eigenvectors) from covariance matrix
262 [ + - ]: 1 : CartVect lambda;
263 [ + - ]: 1 : a.eigen_decomposition( lambda, result.axes );
264 : :
265 : : // Calculate center and extents of box given orientation defined by axes
266 [ + - ]: 1 : return box_from_axes( result, instance, vertices );
267 : : }
268 : :
269 : 22386 : ErrorCode OrientedBox::covariance_data_from_tris( CovarienceData& result, Interface* instance, const Range& elements )
270 : : {
271 : : ErrorCode rval;
272 [ + - ]: 22386 : const Range::iterator begin = elements.lower_bound( CN::TypeDimensionMap[2].first );
273 [ + - ]: 22386 : const Range::iterator end = elements.lower_bound( CN::TypeDimensionMap[3].first );
274 : :
275 : : // compute mean and moments
276 [ + - ][ + - ]: 22386 : result.matrix = Matrix3( 0.0 );
277 [ + - ]: 22386 : result.center = CartVect( 0.0 );
278 : 22386 : result.area = 0.0;
279 [ + - ][ + - ]: 734309 : for( Range::iterator i = begin; i != end; ++i )
[ + + ]
280 : : {
281 : 711923 : const EntityHandle* conn = NULL;
282 : 711923 : int conn_len = 0;
283 [ + - ][ + - ]: 711923 : rval = instance->get_connectivity( *i, conn, conn_len );
284 [ - + ]: 711923 : if( MB_SUCCESS != rval ) return rval;
285 : :
286 : : // for each triangle in the 2-D cell
287 [ + + ]: 1424384 : for( int j = 2; j < conn_len; ++j )
288 : : {
289 : 712461 : EntityHandle vertices[3] = { conn[0], conn[j - 1], conn[j] };
290 [ + - ][ + + ]: 2849844 : CartVect coords[3];
291 [ + - ][ + - ]: 712461 : rval = instance->get_coords( vertices, 3, coords[0].array() );
292 [ - + ]: 712461 : if( MB_SUCCESS != rval ) return rval;
293 : :
294 : : // edge vectors
295 [ + - ]: 712461 : const CartVect edge0 = coords[1] - coords[0];
296 [ + - ]: 712461 : const CartVect edge1 = coords[2] - coords[0];
297 [ + - ][ + - ]: 712461 : const CartVect centroid = ( coords[0] + coords[1] + coords[2] ) / 3;
[ + - ]
298 [ + - ][ + - ]: 712461 : const double tri_area2 = ( edge0 * edge1 ).length();
299 : 712461 : result.area += tri_area2;
300 [ + - ][ + - ]: 712461 : result.center += tri_area2 * centroid;
301 : :
302 : 712461 : result.matrix +=
303 [ + - ][ + - ]: 1424922 : tri_area2 * ( 9 * outer_product( centroid, centroid ) + outer_product( coords[0], coords[0] ) +
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ]
304 [ + - ][ + - ]: 2137383 : outer_product( coords[1], coords[1] ) + outer_product( coords[2], coords[2] ) );
[ + - ]
305 : : } // for each triangle
306 : : } // for each element
307 : :
308 : 22386 : return MB_SUCCESS;
309 : : }
310 : :
311 : 21902 : ErrorCode OrientedBox::compute_from_2d_cells( OrientedBox& result, Interface* instance, const Range& elements )
312 : : {
313 : : // Get orientation data from elements
314 [ + - ]: 21902 : CovarienceData data;
315 [ + - ]: 21902 : ErrorCode rval = covariance_data_from_tris( data, instance, elements );
316 [ - + ]: 21902 : if( MB_SUCCESS != rval ) return rval;
317 : :
318 : : // get vertices from elements
319 [ + - ]: 21902 : Range points;
320 [ + - ]: 21902 : rval = instance->get_adjacencies( elements, 0, false, points, Interface::UNION );
321 [ - + ]: 21902 : if( MB_SUCCESS != rval ) return rval;
322 : :
323 : : // Calculate box given points and orientation data
324 [ + - ]: 21902 : return compute_from_covariance_data( result, instance, data, points );
325 : : }
326 : :
327 : 22701 : ErrorCode OrientedBox::compute_from_covariance_data( OrientedBox& result, Interface* instance, CovarienceData& data,
328 : : const Range& vertices )
329 : : {
330 [ - + ]: 22701 : if( data.area <= 0.0 )
331 : : {
332 [ # # ]: 0 : Matrix3 empty_axes( 0.0 );
333 [ # # ][ # # ]: 0 : result = OrientedBox( empty_axes, CartVect( 0. ) );
[ # # ]
334 : 0 : return MB_SUCCESS;
335 : : }
336 : :
337 : : // get center from sum
338 [ + - ]: 22701 : result.center = data.center / data.area;
339 : :
340 : : // get covariance matrix from moments
341 [ + - ]: 22701 : data.matrix /= 12 * data.area;
342 [ + - ][ + - ]: 22701 : data.matrix -= outer_product( result.center, result.center );
343 : :
344 : : // get axes (Eigenvectors) from covariance matrix
345 [ + - ]: 22701 : CartVect lambda;
346 [ + - ]: 22701 : data.matrix.eigen_decomposition( lambda, result.axes );
347 : :
348 : : // We now have only the axes. Calculate proper center
349 : : // and extents for enclosed points.
350 [ + - ]: 22701 : return box_from_axes( result, instance, vertices );
351 : : }
352 : :
353 : 196 : bool OrientedBox::contained( const CartVect& point, double tol ) const
354 : : {
355 [ + - ]: 196 : CartVect from_center = point - center;
356 : : #if MB_ORIENTED_BOX_UNIT_VECTORS
357 [ + - ][ + - ]: 731 : return fabs( from_center % axes.col( 0 ) ) - length[0] <= tol &&
[ + - ][ + + ]
[ - + ][ # # ]
358 [ + + ][ + - ]: 535 : fabs( from_center % axes.col( 1 ) ) - length[1] <= tol &&
[ + - ][ + - ]
[ + + ][ + + ]
[ # # ]
359 [ + - ][ + - ]: 721 : fabs( from_center % axes.col( 2 ) ) - length[2] <= tol;
[ + - ][ + + ]
[ # # ]
360 : : #else
361 : : for( int i = 0; i < 3; ++i )
362 : : {
363 : : double length = axes.col( i ).length();
364 : : if( fabs( from_center % axes.col( i ) ) - length * length > length * tol ) return false;
365 : : }
366 : : return true;
367 : : #endif
368 : : }
369 : :
370 : 799 : ErrorCode OrientedBox::compute_from_covariance_data( OrientedBox& result, Interface* moab_instance,
371 : : const CovarienceData* data, unsigned data_length,
372 : : const Range& vertices )
373 : : {
374 : : // Sum input CovarienceData structures
375 [ + - ][ + - ]: 799 : CovarienceData data_sum( Matrix3( 0.0 ), CartVect( 0.0 ), 0.0 );
[ + - ]
376 [ + + ]: 2575 : for( const CovarienceData* const end = data + data_length; data != end; ++data )
377 : : {
378 [ + - ]: 1776 : data_sum.matrix += data->matrix;
379 [ + - ]: 1776 : data_sum.center += data->center;
380 : 1776 : data_sum.area += data->area;
381 : : }
382 : : // Compute box from sum of structs
383 [ + - ]: 799 : return compute_from_covariance_data( result, moab_instance, data_sum, vertices );
384 : : }
385 : :
386 : : // bool OrientedBox::contained( const OrientedBox& box, double tol ) const
387 : : //{
388 : : // for (int i = -1; i < 2; i += 2)
389 : : // {
390 : : // for (int j = -1; j < 2; j += 2)
391 : : // {
392 : : // for (int k = -1; k < 2; k += 2)
393 : : // {
394 : : // CartVect corner( center );
395 : : //#ifdef MB_ORIENTED_BOX_UNIT_VECTORS
396 : : // corner += i * box.length[0] * box.axis.col(0);
397 : : // corner += j * box.length[1] * box.axis.col(1);
398 : : // corner += k * box.length[2] * box.axis.col(2);
399 : : //#else
400 : : // corner += i * box.axis.col(0);
401 : : // corner += j * box.axis.col(1);
402 : : // corner += k * box.axis.col(2);
403 : : //#endif
404 : : // if (!contained( corner, tol ))
405 : : // return false;
406 : : // }
407 : : // }
408 : : // }
409 : : // return true;
410 : : //}
411 : :
412 : : /* This is a helper function to check limits on ray length, turning the box-ray
413 : : * intersection test into a box-segment intersection test. Use this to test the
414 : : * limits against one side (plane) of the box. The side of the box (plane) is
415 : : * normal to an axis.
416 : : *
417 : : * normal_par_pos Coordinate of particle's position along axis normal to side of box
418 : : * normal_par_dir Coordinate of particle's direction along axis normal to side of box
419 : : * half_extent Distance between center of box and side of box
420 : : * nonneg_ray_len Maximum ray length in positive direction (in front of origin)
421 : : * neg_ray_len Maximum ray length in negative direction (behind origin)
422 : : * return true if intersection with plane occurs within distance limits
423 : : *
424 : : * ray equation: intersection = origin + dist*direction
425 : : * plane equation: intersection.plane_normal = half_extent
426 : : *
427 : : * Assume plane_normal and direction are unit vectors. Combine equations.
428 : : *
429 : : * (origin + dist*direction).plane_normal = half_extent
430 : : * origin.plane_normal + dist*direction.plane_normal = half_extent
431 : : * dist = (half_extent - origin.plane_normal)/(direction.plane_normal)
432 : : *
433 : : * Although this solves for distance, avoid floating point division.
434 : : *
435 : : * dist*direction.plane_normal = half_extent - origin.plane_normal
436 : : *
437 : : * Use inequalities to test dist against ray length limits. Be aware that
438 : : * inequalities change due to sign of direction.plane_normal.
439 : : */
440 : 6161 : inline bool check_ray_limits( const double normal_par_pos, const double normal_par_dir, const double half_extent,
441 : : const double* nonneg_ray_len, const double* neg_ray_len )
442 : : {
443 : :
444 : 6161 : const double extent_pos_diff = half_extent - normal_par_pos;
445 : :
446 : : // limit in positive direction
447 [ + + ]: 6161 : if( nonneg_ray_len )
448 : : { // should be 0 <= t <= nonneg_ray_len
449 [ - + ]: 5658 : assert( 0 <= *nonneg_ray_len );
450 [ + + ]: 5658 : if( normal_par_dir > 0 )
451 : : { // if/else if needed for pos/neg divisor
452 [ + + ][ + + ]: 4095 : if( *nonneg_ray_len * normal_par_dir >= extent_pos_diff && extent_pos_diff >= 0 ) return true;
453 : : }
454 [ + - ]: 1563 : else if( normal_par_dir < 0 )
455 : : {
456 [ + + ][ + + ]: 1563 : if( *nonneg_ray_len * normal_par_dir <= extent_pos_diff && extent_pos_diff <= 0 ) return true;
457 : : }
458 : : }
459 : : else
460 : : { // should be 0 <= t
461 [ + + ]: 503 : if( normal_par_dir > 0 )
462 : : { // if/else if needed for pos/neg divisor
463 [ + + ]: 449 : if( extent_pos_diff >= 0 ) return true;
464 : : }
465 [ + - ]: 54 : else if( normal_par_dir < 0 )
466 : : {
467 [ + - ]: 54 : if( extent_pos_diff <= 0 ) return true;
468 : : }
469 : : }
470 : :
471 : : // limit in negative direction
472 [ + + ]: 4008 : if( neg_ray_len )
473 : : { // should be neg_ray_len <= t < 0
474 [ - + ]: 3979 : assert( 0 >= *neg_ray_len );
475 [ + + ]: 3979 : if( normal_par_dir > 0 )
476 : : { // if/else if needed for pos/neg divisor
477 [ + + ][ + + ]: 2615 : if( *neg_ray_len * normal_par_dir <= extent_pos_diff && extent_pos_diff < 0 ) return true;
478 : : }
479 [ + - ]: 1364 : else if( normal_par_dir < 0 )
480 : : {
481 [ + + ][ + + ]: 1364 : if( *neg_ray_len * normal_par_dir >= extent_pos_diff && extent_pos_diff > 0 ) return true;
482 : : }
483 : : }
484 : :
485 : 2866 : return false;
486 : : }
487 : :
488 : : /* This implementation copied from cgmMC (overlap.C).
489 : : * Original author: Tim Tautges?
490 : : */
491 : 31642 : bool OrientedBox::intersect_ray( const CartVect& ray_origin, const CartVect& ray_direction, const double reps,
492 : : const double* nonneg_ray_len, const double* neg_ray_len ) const
493 : : {
494 : : // test distance from box center to line
495 [ + - ]: 31642 : const CartVect cx = center - ray_origin;
496 [ + - ]: 31642 : const double dist_s = cx % ray_direction;
497 [ + - ]: 31642 : const double dist_sq = cx % cx - ( dist_s * dist_s );
498 [ + - ]: 31642 : const double max_diagsq = outer_radius_squared( reps );
499 : :
500 : : // For the largest sphere, no intersections exist if discriminant is negative.
501 : : // Geometrically, if distance from box center to line is greater than the
502 : : // longest diagonal, there is no intersection.
503 : : // manipulate the discriminant: 0 > dist_s*dist_s - cx%cx + max_diagsq
504 [ + + ]: 31642 : if( dist_sq > max_diagsq ) return false;
505 : :
506 : : // If the closest possible intersection must be closer than nonneg_ray_len. Be
507 : : // careful with absolute value, squaring distances, and subtracting squared
508 : : // distances.
509 [ + + ]: 25708 : if( nonneg_ray_len )
510 : : {
511 [ - + ]: 24901 : assert( 0 <= *nonneg_ray_len );
512 : : double max_len;
513 [ + + ]: 24901 : if( neg_ray_len )
514 : : {
515 [ - + ]: 16535 : assert( 0 >= *neg_ray_len );
516 [ + - ]: 16535 : max_len = std::max( *nonneg_ray_len, -( *neg_ray_len ) );
517 : : }
518 : : else
519 : : {
520 : 8366 : max_len = *nonneg_ray_len;
521 : : }
522 : 24901 : const double temp = fabs( dist_s ) - max_len;
523 [ + + ][ + + ]: 24901 : if( 0.0 < temp && temp * temp > max_diagsq ) return false;
524 : : }
525 : :
526 : : // if smaller than shortest diagonal, we do hit
527 [ + - ][ + + ]: 25621 : if( dist_sq < inner_radius_squared( reps ) )
528 : : {
529 : : // nonnegative direction
530 [ + + ]: 8503 : if( dist_s >= 0.0 )
531 : : {
532 [ + + ]: 4951 : if( nonneg_ray_len )
533 : : {
534 [ + + ]: 4847 : if( *nonneg_ray_len > dist_s ) return true;
535 : : }
536 : : else
537 : : {
538 : 104 : return true;
539 : : }
540 : : // negative direction
541 : : }
542 : : else
543 : : {
544 [ + + ][ + + ]: 3552 : if( neg_ray_len && *neg_ray_len < dist_s ) return true;
545 : : }
546 : : }
547 : :
548 : : // get transpose of axes
549 [ + - ]: 19681 : Matrix3 B = axes.transpose();
550 : :
551 : : // transform ray to box coordintae system
552 [ + - ][ + - ]: 19681 : CartVect par_pos = B * ( ray_origin - center );
553 [ + - ]: 19681 : CartVect par_dir = B * ray_direction;
554 : :
555 : : // Fast Rejection Test: Ray will not intersect if it is going away from the box.
556 : : // This will not work for rays with neg_ray_len. length[0] is half of box width
557 : : // along axes.col(0).
558 [ + - ]: 19681 : const double half_x = length[0] + reps;
559 [ + - ]: 19681 : const double half_y = length[1] + reps;
560 [ + - ]: 19681 : const double half_z = length[2] + reps;
561 [ + + ]: 19681 : if( !neg_ray_len )
562 : : {
563 [ + - ][ + + ]: 6606 : if( ( par_pos[0] > half_x && par_dir[0] >= 0 ) || ( par_pos[0] < -half_x && par_dir[0] <= 0 ) ) return false;
[ + - ][ + + ]
[ + - ][ + + ]
[ + - ][ + + ]
[ + + ]
564 : :
565 [ + - ][ + + ]: 4574 : if( ( par_pos[1] > half_y && par_dir[1] >= 0 ) || ( par_pos[1] < -half_y && par_dir[1] <= 0 ) ) return false;
[ + - ][ + + ]
[ + - ][ + + ]
[ + - ][ + + ]
[ + + ]
566 : :
567 [ + - ][ + + ]: 4547 : if( ( par_pos[2] > half_z && par_dir[2] >= 0 ) || ( par_pos[2] < -half_z && par_dir[2] <= 0 ) ) return false;
[ + - ][ + + ]
[ + - ][ + + ]
[ + - ][ + + ]
[ + + ]
568 : : }
569 : :
570 : : // test if ray_origin is inside box
571 [ + - ][ + - ]: 58384 : if( par_pos[0] <= half_x && par_pos[0] >= -half_x && par_pos[1] <= half_y && par_pos[1] >= -half_y &&
[ + + ][ + - ]
[ + + ][ + - ]
[ + + ][ + + ]
[ + + ]
572 [ + + ][ + - ]: 40771 : par_pos[2] <= half_z && par_pos[2] >= -half_z )
[ + - ][ + + ]
573 : 8351 : return true;
574 : :
575 : : // test two xy plane
576 [ + - ][ + - ]: 18524 : if( fabs( par_dir[0] * ( half_z - par_pos[2] ) + par_dir[2] * par_pos[0] ) <=
[ + - ][ + - ]
[ + + ]
577 [ + - ][ + + ]: 9881 : fabs( par_dir[2] * half_x ) && // test against x extents using z
578 [ + - ][ + - ]: 619 : fabs( par_dir[1] * ( half_z - par_pos[2] ) + par_dir[2] * par_pos[1] ) <=
[ + - ][ + - ]
579 [ + + ][ + - ]: 10500 : fabs( par_dir[2] * half_y ) && // test against y extents using z
[ + + ]
580 [ + - ][ + - ]: 153 : check_ray_limits( par_pos[2], par_dir[2], half_z, nonneg_ray_len, neg_ray_len ) )
[ + - ]
581 : 78 : return true;
582 [ + - ][ + - ]: 19030 : if( fabs( par_dir[0] * ( -half_z - par_pos[2] ) + par_dir[2] * par_pos[0] ) <= fabs( par_dir[2] * half_x ) &&
[ + - ][ + - ]
[ + - ][ + + ]
[ + + ]
583 [ + + ][ + - ]: 9846 : fabs( par_dir[1] * ( -half_z - par_pos[2] ) + par_dir[2] * par_pos[1] ) <= fabs( par_dir[2] * half_y ) &&
[ + - ][ + - ]
[ + - ][ + - ]
[ + + ]
584 [ + - ][ + - ]: 196 : check_ray_limits( par_pos[2], par_dir[2], -half_z, nonneg_ray_len, neg_ray_len ) )
[ + - ]
585 : 75 : return true;
586 : :
587 : : // test two xz plane
588 [ + - ][ + - ]: 18700 : if( fabs( par_dir[0] * ( half_y - par_pos[1] ) + par_dir[1] * par_pos[0] ) <= fabs( par_dir[1] * half_x ) &&
[ + - ][ + - ]
[ + - ][ + + ]
[ + + ]
589 [ + + ][ + - ]: 9591 : fabs( par_dir[2] * ( half_y - par_pos[1] ) + par_dir[1] * par_pos[2] ) <= fabs( par_dir[1] * half_z ) &&
[ + - ][ + - ]
[ + - ][ + - ]
[ + + ]
590 [ + - ][ + - ]: 283 : check_ray_limits( par_pos[1], par_dir[1], half_y, nonneg_ray_len, neg_ray_len ) )
[ + - ]
591 : 157 : return true;
592 [ + - ][ + - ]: 18678 : if( fabs( par_dir[0] * ( -half_y - par_pos[1] ) + par_dir[1] * par_pos[0] ) <= fabs( par_dir[1] * half_x ) &&
[ + - ][ + - ]
[ + - ][ + + ]
[ + + ]
593 [ + + ][ + - ]: 9726 : fabs( par_dir[2] * ( -half_y - par_pos[1] ) + par_dir[1] * par_pos[2] ) <= fabs( par_dir[1] * half_z ) &&
[ + - ][ + - ]
[ + - ][ + - ]
[ + + ]
594 [ + - ][ + - ]: 582 : check_ray_limits( par_pos[1], par_dir[1], -half_y, nonneg_ray_len, neg_ray_len ) )
[ + - ]
595 : 52 : return true;
596 : :
597 : : // test two yz plane
598 [ + - ][ + - ]: 22495 : if( fabs( par_dir[1] * ( half_x - par_pos[0] ) + par_dir[0] * par_pos[1] ) <= fabs( par_dir[0] * half_y ) &&
[ + - ][ + - ]
[ + - ][ + + ]
[ + + ]
599 [ + + ][ + - ]: 13595 : fabs( par_dir[2] * ( half_x - par_pos[0] ) + par_dir[0] * par_pos[2] ) <= fabs( par_dir[0] * half_z ) &&
[ + - ][ + - ]
[ + - ][ + - ]
[ + + ]
600 [ + - ][ + - ]: 3604 : check_ray_limits( par_pos[0], par_dir[0], half_x, nonneg_ray_len, neg_ray_len ) )
[ + - ]
601 : 2541 : return true;
602 [ + - ][ + - ]: 15097 : if( fabs( par_dir[1] * ( -half_x - par_pos[0] ) + par_dir[0] * par_pos[1] ) <= fabs( par_dir[0] * half_y ) &&
[ + - ][ + - ]
[ + - ][ + + ]
[ + + ]
603 [ + + ][ + - ]: 8738 : fabs( par_dir[2] * ( -half_x - par_pos[0] ) + par_dir[0] * par_pos[2] ) <= fabs( par_dir[0] * half_z ) &&
[ + - ][ + - ]
[ + - ][ + - ]
[ + + ]
604 [ + - ][ + - ]: 1343 : check_ray_limits( par_pos[0], par_dir[0], -half_x, nonneg_ray_len, neg_ray_len ) )
[ + - ]
605 : 392 : return true;
606 : :
607 : 31642 : return false;
608 : : }
609 : :
610 : 0 : ErrorCode OrientedBox::make_hex( EntityHandle& hex, Interface* instance )
611 : : {
612 : : ErrorCode rval;
613 : : int signs[8][3] = { { -1, -1, -1 }, { 1, -1, -1 }, { 1, 1, -1 }, { -1, 1, -1 },
614 : 0 : { -1, -1, 1 }, { 1, -1, 1 }, { 1, 1, 1 }, { -1, 1, 1 } };
615 : :
616 [ # # ]: 0 : std::vector< EntityHandle > vertices;
617 [ # # ]: 0 : for( int i = 0; i < 8; ++i )
618 : : {
619 : 0 : CartVect coords( center );
620 [ # # ]: 0 : for( int j = 0; j < 3; ++j )
621 : : {
622 : : #if MB_ORIENTED_BOX_UNIT_VECTORS
623 [ # # ][ # # ]: 0 : coords += signs[i][j] * ( axes.col( j ) * length[j] );
[ # # ][ # # ]
[ # # ]
624 : : #else
625 : : coords += signs[i][j] * axes.col( j );
626 : : #endif
627 : : }
628 : : EntityHandle handle;
629 [ # # ][ # # ]: 0 : rval = instance->create_vertex( coords.array(), handle );
630 [ # # ]: 0 : if( MB_SUCCESS != rval )
631 : : {
632 [ # # ][ # # ]: 0 : instance->delete_entities( &vertices[0], vertices.size() );
633 : 0 : return rval;
634 : : }
635 [ # # ]: 0 : vertices.push_back( handle );
636 : : }
637 : :
638 [ # # ][ # # ]: 0 : rval = instance->create_element( MBHEX, &vertices[0], vertices.size(), hex );
639 [ # # ]: 0 : if( MB_SUCCESS != rval )
640 : : {
641 [ # # ][ # # ]: 0 : instance->delete_entities( &vertices[0], vertices.size() );
642 : 0 : return rval;
643 : : }
644 : :
645 : 0 : return MB_SUCCESS;
646 : : }
647 : :
648 : 111993 : void OrientedBox::closest_location_in_box( const CartVect& input_position, CartVect& output_position ) const
649 : : {
650 : : // get coordinates on box axes
651 [ + - ]: 111993 : const CartVect from_center = input_position - center;
652 : :
653 : : #if MB_ORIENTED_BOX_UNIT_VECTORS
654 [ + - ][ + - ]: 111993 : CartVect local( from_center % axes.col( 0 ), from_center % axes.col( 1 ), from_center % axes.col( 2 ) );
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ]
655 : :
656 [ + + ]: 447972 : for( int i = 0; i < 3; ++i )
657 : : {
658 [ + - ][ + - ]: 335979 : if( local[i] < -length[i] )
[ + + ]
659 [ + - ][ + - ]: 47559 : local[i] = -length[i];
660 [ + - ][ + - ]: 288420 : else if( local[i] > length[i] )
[ + + ]
661 [ + - ][ + - ]: 51315 : local[i] = length[i];
662 : : }
663 : : #else
664 : : CartVect local( ( from_center % axes.col( 0 ) ) / ( axes.col( 0 ) % axes.col( 0 ) ),
665 : : ( from_center % axes.col( 1 ) ) / ( axes.col( 1 ) % axes.col( 1 ) ),
666 : : ( from_center % axes.col( 2 ) ) / ( axes.col( 2 ) % axes.col( 2 ) ) );
667 : :
668 : : for( int i = 0; i < 3; ++i )
669 : : {
670 : : if( local[i] < -1.0 )
671 : : local[i] = -1.0;
672 : : else if( local[i] > 1.0 )
673 : : local[i] = 1.0;
674 : : }
675 : : #endif
676 : :
677 [ + - ][ + - ]: 111993 : output_position = center + local[0] * axes.col( 0 ) + local[1] * axes.col( 1 ) + local[2] * axes.col( 2 );
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
678 : 111993 : }
679 : :
680 [ + - ][ + - ]: 228 : } // namespace moab
|
