38 static inline void min_max_3(
double a,
double b,
double c,
double& min,
double& max )
67 return fabs( u[0] * v[0] ) + fabs( u[1] * v[1] ) + fabs( u[2] * v[2] );
81 for(
unsigned i = 0; i < 3; ++i )
85 const double t_min =
box_min[i] / seg_unit_dir[i];
86 const double t_max =
box_max[i] / seg_unit_dir[i];
95 if( seg_unit_dir[i] < 0 )
97 if( t_min < seg_end ) seg_end = t_min;
98 if( t_max > seg_start ) seg_start = t_max;
102 if( t_min > seg_start ) seg_start = t_min;
103 if( t_max < seg_end ) seg_end = t_max;
107 return seg_start <= seg_end;
120 else if( a[0] == b[0] )
126 else if( a[1] == b[1] )
155 const double near_zero = 10 * std::numeric_limits< double >::epsilon();
157 if(
first( vertexa, vertexb ) )
161 pip = ray % edge_normal + ray_normal %
edge;
167 pip = ray % edge_normal + ray_normal %
edge;
171 if( near_zero > fabs( pip ) ) pip = 0.0;
177 if( type ) *type = NONE; \
197 const double* nonneg_ray_len,
198 const double* neg_ray_len,
199 const int* orientation,
204 const CartVect rayb = direction * origin;
207 double plucker_coord0 =
plucker_edge_test( vertices[0], vertices[1], raya, rayb );
211 if( orientation && ( *orientation ) * plucker_coord0 > 0 )
217 double plucker_coord1 =
plucker_edge_test( vertices[1], vertices[2], raya, rayb );
223 if( ( *orientation ) * plucker_coord1 > 0 )
230 else if( ( 0.0 < plucker_coord0 && 0.0 > plucker_coord1 ) || ( 0.0 > plucker_coord0 && 0.0 < plucker_coord1 ) )
236 double plucker_coord2 =
plucker_edge_test( vertices[2], vertices[0], raya, rayb );
242 if( ( *orientation ) * plucker_coord2 > 0 )
249 else if( ( 0.0 < plucker_coord1 && 0.0 > plucker_coord2 ) || ( 0.0 > plucker_coord1 && 0.0 < plucker_coord2 ) ||
250 ( 0.0 < plucker_coord0 && 0.0 > plucker_coord2 ) || ( 0.0 > plucker_coord0 && 0.0 < plucker_coord2 ) )
256 if( 0.0 == plucker_coord0 && 0.0 == plucker_coord1 && 0.0 == plucker_coord2 )
262 const double inverse_sum = 1.0 / ( plucker_coord0 + plucker_coord1 + plucker_coord2 );
263 assert( 0.0 != inverse_sum );
264 const CartVect intersection( plucker_coord0 * inverse_sum * vertices[2] +
265 plucker_coord1 * inverse_sum * vertices[0] +
266 plucker_coord2 * inverse_sum * vertices[1] );
270 double max_abs_dir = 0;
271 for(
unsigned int i = 0; i < 3; ++i )
273 if( fabs( direction[i] ) > max_abs_dir )
276 max_abs_dir = fabs( direction[i] );
279 const double dist = ( intersection[idx] - origin[idx] ) / direction[idx];
282 if( ( nonneg_ray_len && *nonneg_ray_len < dist ) ||
283 ( neg_ray_len && *neg_ray_len >= dist ) ||
284 ( !neg_ray_len && 0 > dist ) )
292 *type =
type_list[( ( 0.0 == plucker_coord2 ) << 2 ) + ( ( 0.0 == plucker_coord1 ) << 1 ) +
293 ( 0.0 == plucker_coord0 )];
303 const double* ray_length )
305 const CartVect p0 = vertices[0] - vertices[1];
306 const CartVect p1 = vertices[0] - vertices[2];
310 const double mP = p0 % c;
311 const double betaP = p % c;
314 if( betaP < 0 )
return false;
318 if( betaP > 0 )
return false;
326 double gammaP = v % d;
329 if( gammaP < 0 || betaP + gammaP > mP )
return false;
331 else if( betaP + gammaP < mP || gammaP > 0 )
334 const double tP = p1 % d;
335 const double m = 1.0 / mP;
336 const double beta = betaP * m;
337 const double gamma = gammaP * m;
338 const double t = -tP * m;
339 if( ray_length && t > *ray_length )
return false;
341 if( beta < 0 || gamma < 0 || beta + gamma > 1 || t < 0.0 )
return false;
354 const double epsilon = 1e-12;
359 t_exit = std::numeric_limits< double >::infinity();
363 bool ray_is_valid =
false;
364 for(
int axis = 0; axis < 3; ++axis )
366 if( fabs( ray_dir[axis] ) < epsilon )
368 if( ray_pt[axis] >=
box_min[axis] && ray_pt[axis] <=
box_max[axis] )
376 t1 = (
box_min[axis] - ray_pt[axis] ) / ray_dir[axis];
377 t2 = (
box_max[axis] - ray_pt[axis] ) / ray_dir[axis];
386 if( t_enter < t1 ) t_enter = t1;
387 if( t_exit > t2 ) t_exit = t2;
391 if( t_enter < t2 ) t_enter = t2;
392 if( t_exit > t1 ) t_exit = t1;
396 return ray_is_valid && ( t_enter <= t_exit );
401 if( normal[0] < 0.0 ) std::swap( min[0], max[0] );
402 if( normal[1] < 0.0 ) std::swap( min[1], max[1] );
403 if( normal[2] < 0.0 ) std::swap( min[2], max[2] );
405 return ( normal % min <= -d ) && ( normal % max >= -d );
408 #define CHECK_RANGE( A, B, R ) \
409 if( ( A ) < ( B ) ) \
411 if( ( A ) > ( R ) || ( B ) < -( R ) ) return false; \
413 else if( ( B ) > ( R ) || ( A ) < -( R ) ) \
428 const CartVect v0( vertices[0] - box_center );
429 const CartVect v1( vertices[1] - box_center );
430 const CartVect v2( vertices[2] - box_center );
433 if( v0[0] > box_dims[0] && v1[0] > box_dims[0] && v2[0] > box_dims[0] )
return false;
434 if( v0[1] > box_dims[1] && v1[1] > box_dims[1] && v2[1] > box_dims[1] )
return false;
435 if( v0[2] > box_dims[2] && v1[2] > box_dims[2] && v2[2] > box_dims[2] )
return false;
436 if( v0[0] < -box_dims[0] && v1[0] < -box_dims[0] && v2[0] < -box_dims[0] )
return false;
437 if( v0[1] < -box_dims[1] && v1[1] < -box_dims[1] && v2[1] < -box_dims[1] )
return false;
438 if( v0[2] < -box_dims[2] && v1[2] < -box_dims[2] && v2[2] < -box_dims[2] )
return false;
441 const CartVect e0( vertices[1] - vertices[0] );
442 const CartVect e1( vertices[2] - vertices[1] );
443 const CartVect e2( vertices[0] - vertices[2] );
446 double fex, fey, fez, p0, p1, p2, rad;
451 p0 = e0[2] * v0[1] - e0[1] * v0[2];
452 p2 = e0[2] * v2[1] - e0[1] * v2[2];
453 rad = fez * box_dims[1] + fey * box_dims[2];
456 p0 = -e0[2] * v0[0] + e0[0] * v0[2];
457 p2 = -e0[2] * v2[0] + e0[0] * v2[2];
458 rad = fez * box_dims[0] + fex * box_dims[2];
461 p1 = e0[1] * v1[0] - e0[0] * v1[1];
462 p2 = e0[1] * v2[0] - e0[0] * v2[1];
463 rad = fey * box_dims[0] + fex * box_dims[1];
470 p0 = e1[2] * v0[1] - e1[1] * v0[2];
471 p2 = e1[2] * v2[1] - e1[1] * v2[2];
472 rad = fez * box_dims[1] + fey * box_dims[2];
475 p0 = -e1[2] * v0[0] + e1[0] * v0[2];
476 p2 = -e1[2] * v2[0] + e1[0] * v2[2];
477 rad = fez * box_dims[0] + fex * box_dims[2];
480 p0 = e1[1] * v0[0] - e1[0] * v0[1];
481 p1 = e1[1] * v1[0] - e1[0] * v1[1];
482 rad = fey * box_dims[0] + fex * box_dims[1];
489 p0 = e2[2] * v0[1] - e2[1] * v0[2];
490 p1 = e2[2] * v1[1] - e2[1] * v1[2];
491 rad = fez * box_dims[1] + fey * box_dims[2];
494 p0 = -e2[2] * v0[0] + e2[0] * v0[2];
495 p1 = -e2[2] * v1[0] + e2[0] * v1[2];
496 rad = fez * box_dims[0] + fex * box_dims[2];
499 p1 = e2[1] * v1[0] - e2[0] * v1[1];
500 p2 = e2[1] * v2[0] - e2[0] * v2[1];
501 rad = fey * box_dims[0] + fex * box_dims[1];
514 const CartVect box_center = 0.5 * ( box_max_corner + box_min_corner );
515 const CartVect box_hf_dim = 0.5 * ( box_max_corner - box_min_corner );
520 EntityType elem_type,
536 vt[0] = elem_corners[0];
537 vt[1] = elem_corners[1];
538 for(
int j = 2; j < nodecount; j++ )
540 vt[2] = elem_corners[j];
556 return ( -v1 + v2 + v3 - v4 ) * ( -v1 - v2 + v3 + v4 );
561 return ( v2 - v1 ) * ( v3 - v1 );
571 assert( num_corner <=
sizeof( corners ) /
sizeof( corners[0] ) );
572 for(
unsigned i = 0; i < num_corner; ++i )
573 corners[i] = elem_corners[i] - box_center;
596 int not_less[3] = { num_corner, num_corner, num_corner };
597 int not_greater[3] = { num_corner, num_corner, num_corner };
599 for( i = 0; i < num_corner; ++i )
603 if( elem_corners[i][0] < -dims[0] )
605 else if( elem_corners[i][0] > dims[0] )
610 if( elem_corners[i][1] < -dims[1] )
612 else if( elem_corners[i][1] > dims[1] )
617 if( elem_corners[i][2] < -dims[2] )
619 else if( elem_corners[i][2] > dims[2] )
624 if( !not_inside )
return true;
629 if( not_greater[0] * not_greater[1] * not_greater[2] * not_less[0] * not_less[1] * not_less[2] == 0 )
638 for( e = 0; e < num_edge; ++e )
647 cross[0] = elem_corners[indices[0]][2] - elem_corners[indices[1]][2];
648 cross[1] = elem_corners[indices[1]][1] - elem_corners[indices[0]][1];
652 dot = fabs(
cross[0] * dims[1] ) + fabs(
cross[1] * dims[2] );
653 not_less[0] = not_greater[0] = num_corner - 1;
654 for( i = ( indices[0] + 1 ) % num_corner; i != indices[0]; i = ( i + 1 ) % num_corner )
656 tmp =
cross[0] * elem_corners[i][1] +
cross[1] * elem_corners[i][2];
657 not_less[0] -= ( tmp < -
dot );
658 not_greater[0] -= ( tmp >
dot );
661 if( not_less[0] * not_greater[0] == 0 )
return false;
668 cross[0] = elem_corners[indices[0]][0] - elem_corners[indices[1]][0];
669 cross[1] = elem_corners[indices[1]][2] - elem_corners[indices[0]][2];
673 dot = fabs(
cross[0] * dims[2] ) + fabs(
cross[1] * dims[0] );
674 not_less[0] = not_greater[0] = num_corner - 1;
675 for( i = ( indices[0] + 1 ) % num_corner; i != indices[0]; i = ( i + 1 ) % num_corner )
677 tmp =
cross[0] * elem_corners[i][2] +
cross[1] * elem_corners[i][0];
678 not_less[0] -= ( tmp < -
dot );
679 not_greater[0] -= ( tmp >
dot );
682 if( not_less[0] * not_greater[0] == 0 )
return false;
689 cross[0] = elem_corners[indices[0]][1] - elem_corners[indices[1]][1];
690 cross[1] = elem_corners[indices[1]][0] - elem_corners[indices[0]][0];
694 dot = fabs(
cross[0] * dims[0] ) + fabs(
cross[1] * dims[1] );
695 not_less[0] = not_greater[0] = num_corner - 1;
696 for( i = ( indices[0] + 1 ) % num_corner; i != indices[0]; i = ( i + 1 ) % num_corner )
698 tmp =
cross[0] * elem_corners[i][0] +
cross[1] * elem_corners[i][1];
699 not_less[0] -= ( tmp < -
dot );
700 not_greater[0] -= ( tmp >
dot );
703 if( not_less[0] * not_greater[0] == 0 )
return false;
709 for( f = 0; f < num_face; ++f )
715 norm =
tri_norm( elem_corners[indices[0]], elem_corners[indices[1]], elem_corners[indices[2]] );
718 norm =
quad_norm( elem_corners[indices[0]], elem_corners[indices[1]], elem_corners[indices[2]],
719 elem_corners[indices[3]] );
729 not_less[0] = not_greater[0] = num_corner;
730 for( i = 0; i < num_corner; ++i )
732 tmp = norm % elem_corners[i];
733 not_less[0] -= ( tmp < -
dot );
734 not_greater[0] -= ( tmp >
dot );
737 if( not_less[0] * not_greater[0] == 0 )
return false;
763 int not_less[3] = { 8, 8, 8 };
764 int not_greater[3] = { 8, 8, 8 };
766 for( i = 0; i < 8; ++i )
770 if( corners[i][0] < -dims[0] )
772 else if( corners[i][0] > dims[0] )
777 if( corners[i][1] < -dims[1] )
779 else if( corners[i][1] > dims[1] )
784 if( corners[i][2] < -dims[2] )
786 else if( corners[i][2] > dims[2] )
791 if( !not_inside )
return true;
796 if( not_greater[0] * not_greater[1] * not_greater[2] * not_less[0] * not_less[1] * not_less[2] == 0 )
800 const unsigned edges[12][2] = { { 0, 1 }, { 0, 4 }, { 0, 3 }, { 2, 3 }, { 2, 1 }, { 2, 6 },
801 { 5, 6 }, { 5, 1 }, { 5, 4 }, { 7, 4 }, { 7, 3 }, { 7, 6 } };
806 for( e = 0; e < 12; ++e )
809 const CartVect& v0 = corners[edges[e][0]];
810 const CartVect& v1 = corners[edges[e][1]];
816 cross[0] = v0[2] - v1[2];
817 cross[1] = v1[1] - v0[1];
821 dot = fabs(
cross[0] * dims[1] ) + fabs(
cross[1] * dims[2] );
822 not_less[0] = not_greater[0] = 7;
823 for( i = ( edges[e][0] + 1 ) % 8; i != edges[e][0]; i = ( i + 1 ) % 8 )
825 tmp =
cross[0] * corners[i][1] +
cross[1] * corners[i][2];
826 not_less[0] -= ( tmp < -
dot );
827 not_greater[0] -= ( tmp >
dot );
830 if( not_less[0] * not_greater[0] == 0 )
return false;
837 cross[0] = v0[0] - v1[0];
838 cross[1] = v1[2] - v0[2];
842 dot = fabs(
cross[0] * dims[2] ) + fabs(
cross[1] * dims[0] );
843 not_less[0] = not_greater[0] = 7;
844 for( i = ( edges[e][0] + 1 ) % 8; i != edges[e][0]; i = ( i + 1 ) % 8 )
846 tmp =
cross[0] * corners[i][2] +
cross[1] * corners[i][0];
847 not_less[0] -= ( tmp < -
dot );
848 not_greater[0] -= ( tmp >
dot );
851 if( not_less[0] * not_greater[0] == 0 )
return false;
858 cross[0] = v0[1] - v1[1];
859 cross[1] = v1[0] - v0[0];
863 dot = fabs(
cross[0] * dims[0] ) + fabs(
cross[1] * dims[1] );
864 not_less[0] = not_greater[0] = 7;
865 for( i = ( edges[e][0] + 1 ) % 8; i != edges[e][0]; i = ( i + 1 ) % 8 )
867 tmp =
cross[0] * corners[i][0] +
cross[1] * corners[i][1];
868 not_less[0] -= ( tmp < -
dot );
869 not_greater[0] -= ( tmp >
dot );
872 if( not_less[0] * not_greater[0] == 0 )
return false;
877 const unsigned faces[6][4] = { { 0, 1, 2, 3 }, { 0, 1, 5, 4 }, { 1, 2, 6, 5 },
878 { 2, 6, 7, 3 }, { 3, 7, 4, 0 }, { 7, 4, 5, 6 } };
879 for( f = 0; f < 6; ++f )
881 norm =
quad_norm( corners[faces[f][0]], corners[faces[f][1]], corners[faces[f][2]], corners[faces[f][3]] );
886 not_less[0] = not_greater[0] = 8;
887 for( i = 0; i < 8; ++i )
889 tmp = norm % corners[i];
890 not_less[0] -= ( tmp < -
dot );
891 not_greater[0] -= ( tmp >
dot );
894 if( not_less[0] * not_greater[0] == 0 )
return false;
907 double dot, dot1, dot2, dot3, min, max;
910 if( fabs(
edge[1] *
edge[2] ) > std::numeric_limits< double >::epsilon() )
912 dot = fabs(
edge[2] ) * dims[1] + fabs(
edge[1] ) * dims[2];
913 dot1 =
edge[2] * ve[1] -
edge[1] * ve[2];
914 dot2 =
edge[2] * v1[1] -
edge[1] * v1[2];
915 dot3 =
edge[2] * v2[1] -
edge[1] * v2[2];
917 if( max < -dot || min >
dot )
return false;
921 if( fabs(
edge[1] *
edge[2] ) > std::numeric_limits< double >::epsilon() )
923 dot = fabs(
edge[2] ) * dims[0] + fabs(
edge[0] ) * dims[2];
924 dot1 = -
edge[2] * ve[0] +
edge[0] * ve[2];
925 dot2 = -
edge[2] * v1[0] +
edge[0] * v1[2];
926 dot3 = -
edge[2] * v2[0] +
edge[0] * v2[2];
928 if( max < -dot || min >
dot )
return false;
932 if( fabs(
edge[1] *
edge[2] ) > std::numeric_limits< double >::epsilon() )
934 dot = fabs(
edge[1] ) * dims[0] + fabs(
edge[0] ) * dims[1];
935 dot1 =
edge[1] * ve[0] -
edge[0] * ve[1];
936 dot2 =
edge[1] * v1[0] -
edge[0] * v1[1];
937 dot3 =
edge[1] * v2[0] -
edge[0] * v2[1];
939 if( max < -dot || min >
dot )
return false;
961 if( fabs( corners[0][0] ) <= dims[0] && fabs( corners[0][1] ) <= dims[1] && fabs( corners[0][2] ) <= dims[2] )
963 if( fabs( corners[1][0] ) <= dims[0] && fabs( corners[1][1] ) <= dims[1] && fabs( corners[1][2] ) <= dims[2] )
965 if( fabs( corners[2][0] ) <= dims[0] && fabs( corners[2][1] ) <= dims[1] && fabs( corners[2][2] ) <= dims[2] )
967 if( fabs( corners[3][0] ) <= dims[0] && fabs( corners[3][1] ) <= dims[1] && fabs( corners[3][2] ) <= dims[2] )
972 if( corners[0][0] < -dims[0] && corners[1][0] < -dims[0] && corners[2][0] < -dims[0] &&
973 corners[3][0] < -dims[0] )
975 if( corners[0][0] > dims[0] && corners[1][0] > dims[0] && corners[2][0] > dims[0] && corners[3][0] > dims[0] )
978 if( corners[0][1] < -dims[1] && corners[1][1] < -dims[1] && corners[2][1] < -dims[1] &&
979 corners[3][1] < -dims[1] )
981 if( corners[0][1] > dims[1] && corners[1][1] > dims[1] && corners[2][1] > dims[1] && corners[3][1] > dims[1] )
984 if( corners[0][2] < -dims[2] && corners[1][2] < -dims[2] && corners[2][2] < -dims[2] &&
985 corners[3][2] < -dims[2] )
987 if( corners[0][2] > dims[2] && corners[1][2] > dims[2] && corners[2][2] > dims[2] && corners[3][2] > dims[2] )
992 double dot, dot1, dot2;
994 const CartVect v01 = corners[1] - corners[0];
995 const CartVect v02 = corners[2] - corners[0];
998 dot1 = norm % corners[0];
999 dot2 = norm % corners[3];
1000 if( dot1 > dot2 ) std::swap( dot1, dot2 );
1001 if( dot2 < -dot || dot1 >
dot )
return false;
1003 const CartVect v03 = corners[3] - corners[0];
1006 dot1 = norm % corners[0];
1007 dot2 = norm % corners[2];
1008 if( dot1 > dot2 ) std::swap( dot1, dot2 );
1009 if( dot2 < -dot || dot1 >
dot )
return false;
1013 dot1 = norm % corners[0];
1014 dot2 = norm % corners[1];
1015 if( dot1 > dot2 ) std::swap( dot1, dot2 );
1016 if( dot2 < -dot || dot1 >
dot )
return false;
1018 const CartVect v12 = corners[2] - corners[1];
1019 const CartVect v13 = corners[3] - corners[1];
1022 dot1 = norm % corners[0];
1023 dot2 = norm % corners[1];
1024 if( dot1 > dot2 ) std::swap( dot1, dot2 );
1025 if( dot2 < -dot || dot1 >
dot )
return false;
1029 const CartVect v23 = corners[3] - corners[2];
1062 const CartVect sv( vertices[1] - vertices[0] );
1063 const CartVect tv( vertices[2] - vertices[0] );
1064 const CartVect pv( vertices[0] - location );
1065 const double ss = sv % sv;
1066 const double st = sv % tv;
1067 const double tt = tv % tv;
1068 const double sp = sv % pv;
1069 const double tp = tv % pv;
1070 const double det = ss * tt - st * st;
1071 double s = st * tp - tt * sp;
1072 double t = st * sp - ss * tp;
1083 closest_out = vertices[1];
1085 closest_out = vertices[0] - ( sp / ss ) * sv;
1090 closest_out = vertices[2];
1092 closest_out = vertices[0] - ( tp / tt ) * tv;
1096 closest_out = vertices[0];
1103 closest_out = vertices[0];
1104 else if( -tp >= tt )
1105 closest_out = vertices[2];
1107 closest_out = vertices[0] - ( tp / tt ) * tv;
1114 closest_out = vertices[0];
1115 else if( -sp >= ss )
1116 closest_out = vertices[1];
1118 closest_out = vertices[0] - ( sp / ss ) * sv;
1123 const double inv_det = 1.0 / det;
1126 closest_out = vertices[0] + s * sv + t * tv;
1138 const double num = t - s;
1139 const double den = ss - 2 * st + tt;
1141 closest_out = vertices[1];
1146 closest_out = s * vertices[1] + t * vertices[2];
1150 closest_out = vertices[2];
1152 closest_out = vertices[0];
1154 closest_out = vertices[0] - ( tp / tt ) * tv;
1163 const double num = t - s;
1164 const double den = tt - 2 * st + ss;
1166 closest_out = vertices[2];
1171 closest_out = s * vertices[1] + t * vertices[2];
1175 closest_out = vertices[1];
1177 closest_out = vertices[0];
1179 closest_out = vertices[0] - ( sp / ss ) * sv;
1184 const double num = tt + tp - st - sp;
1187 closest_out = vertices[2];
1191 const double den = ss - 2 * st + tt;
1193 closest_out = vertices[1];
1198 closest_out = s * vertices[1] + t * vertices[2];
1218 for( i = 0; i < 3; ++i )
1220 pv[i] = vertices[i] - closest_out;
1221 if( ( pv[i] % pv[i] ) <= tsqr )
1228 for( i = 0; i < 3; ++i )
1230 ev = vertices[( i + 1 ) % 3] - vertices[i];
1231 t = ( ev % pv[i] ) / ( ev % ev );
1232 ep = closest_out - ( vertices[i] + t * ev );
1233 if( ( ep % ep ) <= tsqr )
1235 closest_topo = i + 3;
1249 const int n = num_vertices;
1251 double shortest_sqr, dist_sqr, t_closest, t;
1256 v = vertices[0] - vertices[e];
1257 t_closest = ( v % ( location - vertices[e] ) ) / ( v % v );
1258 if( t_closest < 0.0 )
1259 d = location - vertices[e];
1260 else if( t_closest > 1.0 )
1261 d = location - vertices[0];
1263 d = location - vertices[e] - t_closest * v;
1264 shortest_sqr = d % d;
1265 for( i = 0; i < n - 1; ++i )
1267 v = vertices[i + 1] - vertices[i];
1268 t = ( v % ( location - vertices[i] ) ) / ( v % v );
1270 d = location - vertices[i];
1272 d = location - vertices[i + 1];
1274 d = location - vertices[i] - t * v;
1276 if( dist_sqr < shortest_sqr )
1279 shortest_sqr = dist_sqr;
1286 if( t_closest <= 0.0 )
1288 closest_out = vertices[e];
1291 else if( t_closest >= 1.0 )
1293 closest_out = vertices[( e + 1 ) % n];
1298 const CartVect v0 = vertices[e] - vertices[( e + n - 1 ) % n];
1299 const CartVect v1 = vertices[( e + 1 ) % n] - vertices[e];
1300 const CartVect v2 = vertices[( e + 2 ) % n] - vertices[( e + 1 ) % n];
1301 const CartVect norm = ( 1.0 - t_closest ) * ( v0 * v1 ) + t_closest * ( v1 * v2 );
1303 if( ( norm % ( ( vertices[e] - location ) * v1 ) ) <= 0.0 )
1305 closest_out = vertices[e] + t_closest * v1;
1311 const double D = -( norm % ( vertices[e] + t_closest * v1 ) );
1312 closest_out = ( location - ( norm % location + D ) * norm ) / ( norm % norm );
1317 closest[0] = point[0] < min[0] ? min[0] : point[0] > max[0] ? max[0] : point[0];
1318 closest[1] = point[1] < min[1] ? min[1] : point[1] > max[1] ? max[1] : point[1];
1319 closest[2] = point[2] < min[2] ? min[2] : point[2] > max[2] ? max[2] : point[2];
1340 for(
int k = 0; k < 3; k++ )
1342 double b1min = box_min1[k], b1max = box_max1[k];
1343 double b2min = box_min2[k], b2max = box_max2[k];
1344 if( b1min -
tolerance > b2max )
return false;
1345 if( b2min -
tolerance > b1max )
return false;
1353 assert( num1 >= 1 && num2 >= 1 );
1354 CartVect box_min1 = list1[0], box_max1 = list1[0];
1355 CartVect box_min2 = list2[0], box_max2 = list2[0];
1356 for(
int i = 1; i < num1; i++ )
1358 for(
int k = 0; k < 3; k++ )
1360 double val = list1[i][k];
1361 if( box_min1[k] > val ) box_min1[k] = val;
1362 if( box_max1[k] < val ) box_max1[k] = val;
1365 for(
int i = 1; i < num2; i++ )
1367 for(
int k = 0; k < 3; k++ )
1369 double val = list2[i][k];
1370 if( box_min2[k] > val ) box_min2[k] = val;
1371 if( box_max2[k] < val ) box_max2[k] = val;
1396 double bmin11, bmax11, bmin12, bmax12;
1397 bmin11 = bmax11 = list1[0];
1398 bmin12 = bmax12 = list1[1];
1400 double bmin21, bmax21, bmin22, bmax22;
1401 bmin21 = bmax21 = list2[0];
1402 bmin22 = bmax22 = list2[1];
1404 for(
int i = 1; i < num1; i++ )
1406 if( bmin11 > list1[2 * i] ) bmin11 = list1[2 * i];
1407 if( bmax11 < list1[2 * i] ) bmax11 = list1[2 * i];
1408 if( bmin12 > list1[2 * i + 1] ) bmin12 = list1[2 * i + 1];
1409 if( bmax12 < list1[2 * i + 1] ) bmax12 = list1[2 * i + 1];
1411 for(
int i = 1; i < num2; i++ )
1413 if( bmin21 > list2[2 * i] ) bmin21 = list2[2 * i];
1414 if( bmax21 < list2[2 * i] ) bmax21 = list2[2 * i];
1415 if( bmin22 > list2[2 * i + 1] ) bmin22 = list2[2 * i + 1];
1416 if( bmax22 < list2[2 * i + 1] ) bmax22 = list2[2 * i + 1];
1419 if( ( bmax11 < bmin21 +
tolerance ) || ( bmax21 < bmin11 +
tolerance ) )
return false;
1421 if( ( bmax12 < bmin22 +
tolerance ) || ( bmax22 < bmin12 +
tolerance ) )
return false;
1442 const double error_tol_sqr = tol * tol;
1447 while( delta % delta > error_tol_sqr )
1451 if( det < std::numeric_limits< double >::epsilon() )
return false;
1472 const double LinearHexMap::corner_xi[8][3] = { { -1, -1, -1 }, { 1, -1, -1 }, { 1, 1, -1 }, { -1, 1, -1 },
1473 { -1, -1, 1 }, { 1, -1, 1 }, { 1, 1, 1 }, { -1, 1, 1 } };
1482 for(
unsigned i = 0; i < 8; ++i )
1495 for(
unsigned i = 0; i < 8; ++i )
1497 const double xi_p = 1 + xi[0] *
corner_xi[i][0];
1498 const double eta_p = 1 + xi[1] *
corner_xi[i][1];
1499 const double zeta_p = 1 + xi[2] *
corner_xi[i][2];
1500 const double dNi_dxi =
corner_xi[i][0] * eta_p * zeta_p;
1501 const double dNi_deta =
corner_xi[i][1] * xi_p * zeta_p;
1502 const double dNi_dzeta =
corner_xi[i][2] * xi_p * eta_p;
1503 J( 0, 0 ) += dNi_dxi *
corners[i][0];
1504 J( 1, 0 ) += dNi_dxi *
corners[i][1];
1505 J( 2, 0 ) += dNi_dxi *
corners[i][2];
1506 J( 0, 1 ) += dNi_deta *
corners[i][0];
1507 J( 1, 1 ) += dNi_deta *
corners[i][1];
1508 J( 2, 1 ) += dNi_deta *
corners[i][2];
1509 J( 0, 2 ) += dNi_dzeta *
corners[i][0];
1510 J( 1, 2 ) += dNi_dzeta *
corners[i][1];
1511 J( 2, 2 ) += dNi_dzeta *
corners[i][2];
1525 fabs( xi[2] ) - 1 < etol;