Branch data Line data Source code
1 : : // file: IntersectionTool.cpp
2 : : // author: Michael Stephenson
3 : : //
4 : :
5 : : #include <math.h>
6 : : #include "IntersectionTool.hpp"
7 : : #include "CubitVector.hpp"
8 : : #include "CubitMessage.hpp"
9 : : #include "GeometryDefines.h"
10 : : #include "DLIList.hpp"
11 : :
12 : :
13 : :
14 : : // double IntersectionTool::distance_point_line(const double point[3],
15 : : // const double start[3],
16 : : // const double end[3],
17 : : // double &t)
18 : : // {
19 : : // int i;
20 : : // double distSq = 0.0;
21 : : // for (i = 0; i < 3; i++)
22 : : // distSq += (point[i] - end[i]) * (point[i] - end[i]);
23 : : // if (distSq < mTolerance) {
24 : : // t = 1.0;
25 : : // return 0.0;
26 : : // }
27 : :
28 : : // distSq = 0.0;
29 : : // for (i = 0; i < 3; i++)
30 : : // distSq += (point[i] - start[i]) * (point[i] - start[i]);
31 : : // if (distSq < mTolerance) {
32 : : // t = 0.0;
33 : : // return 0.0;
34 : : // }
35 : :
36 : : // double div = 0.0;
37 : : // for (i = 0; i < 3; i++)
38 : : // div += (end[i] - start[i]) * (end[i] - start[i]);
39 : : // if (div < mTolerance) {
40 : : // return -1.0;
41 : : // }
42 : :
43 : : // t = sqrt(distSq)/div;
44 : :
45 : : // double pt[3];
46 : : // for (i = 0; i < 3; i++)
47 : : // pt[i] = (end[i] - start[i]) * t + start[i];
48 : :
49 : : // distSq = 0;
50 : : // for (i = 0; i < 3; i++)
51 : : // distSq += (point[i] - pt[i]) * (point[i] - pt[i]);
52 : :
53 : : // return sqrt(distSq);
54 : : // }
55 : :
56 : 0 : double IntersectionTool::parametric_position(const double node[3],
57 : : const double pt1[3],
58 : : const double pt2[3])
59 : : {
60 : : int i;
61 : : double dist_sq, t;
62 : :
63 : : // check for end-points
64 : : double p13[3];
65 [ # # ]: 0 : for (i = 0; i < 3; i++)
66 : 0 : p13[i] = node[i] - pt1[i];
67 : 0 : dist_sq = p13[0] * p13[0] + p13[1] * p13[1] + p13[2] * p13[2];
68 [ # # ]: 0 : if (dist_sq < mToleranceSquared) {
69 : 0 : t = 0.0;
70 : 0 : return t;
71 : : }
72 : :
73 : : double p23[3];
74 [ # # ]: 0 : for (i = 0; i < 3; i++)
75 : 0 : p23[i] = node[i] - pt2[i];
76 : 0 : dist_sq = p23[0] * p23[0] + p23[1] * p23[1] + p23[2] * p23[2];
77 [ # # ]: 0 : if (dist_sq < mToleranceSquared) {
78 : 0 : t = 1.0;
79 : 0 : return t;
80 : : }
81 : :
82 : : // t is parametric distance along vector p12
83 : : double p12[3];
84 [ # # ]: 0 : for (i = 0; i < 3; i++)
85 : 0 : p12[i] = pt2[i] - pt1[i];
86 : :
87 : : // point1 and point2 are coincident if dot1 is zero
88 : 0 : double dot1 = p12[0] * p12[0] + p12[1] * p12[1] + p12[2] * p12[2];
89 [ # # ][ # # ]: 0 : if (dot1 > -mToleranceSquared && dot1 < mToleranceSquared)
90 : 0 : return CUBIT_DBL_MAX;
91 : 0 : t = (p13[0] * p12[0] + p13[1] * p12[1] + p13[2] * p12[2]) / dot1;
92 [ # # ][ # # ]: 0 : if (t > -mTolerance && t < mTolerance)
93 : 0 : t = 0.0;
94 [ # # ][ # # ]: 0 : else if ((t - 1.0) > -mTolerance &&
95 : 0 : (t - 1.0) < mTolerance)
96 : 0 : t = 1.0;
97 : 0 : return t;
98 : : }
99 : :
100 : 0 : int IntersectionTool::point_on_polyline(CubitVector& pt, DLIList<CubitVector*> &pt_list,
101 : : double *tol_in)
102 : : {
103 : : int i, ret;
104 : : double t, distance;
105 : : double pt_coords[3];
106 : : double line_coords1[3];
107 : : double line_coords2[3];
108 : 0 : double tol = GEOMETRY_RESABS;
109 : :
110 [ # # ]: 0 : if(tol_in)
111 : 0 : tol = *tol_in;
112 : :
113 : 0 : ret = 0;
114 : :
115 [ # # ]: 0 : pt_coords[0] = pt.x();
116 [ # # ]: 0 : pt_coords[1] = pt.y();
117 [ # # ]: 0 : pt_coords[2] = pt.z();
118 : :
119 [ # # ]: 0 : pt_list.reset();
120 [ # # ]: 0 : CubitVector *last_pt = pt_list.get_and_step();
121 [ # # ][ # # ]: 0 : for(i=pt_list.size(); i>1; i--)
122 : : {
123 [ # # ]: 0 : CubitVector *next_pt = pt_list.get_and_step();
124 : :
125 [ # # ]: 0 : line_coords1[0] = last_pt->x();
126 [ # # ]: 0 : line_coords1[1] = last_pt->y();
127 [ # # ]: 0 : line_coords1[2] = last_pt->z();
128 [ # # ]: 0 : line_coords2[0] = next_pt->x();
129 [ # # ]: 0 : line_coords2[1] = next_pt->y();
130 [ # # ]: 0 : line_coords2[2] = next_pt->z();
131 : :
132 [ # # ]: 0 : distance = distance_point_line(pt_coords, line_coords1, line_coords2, t);
133 : :
134 [ # # ][ # # ]: 0 : if(distance > -tol && distance < tol)
135 : : {
136 : 0 : i = 1;
137 : 0 : ret = 1;
138 : : }
139 : : else
140 : 0 : last_pt = next_pt;
141 : : }
142 : 0 : return ret;
143 : : }
144 : :
145 : 0 : double IntersectionTool::distance_point_line(const double node[3],
146 : : const double pt1[3],
147 : : const double pt2[3],
148 : : double &t)
149 : : {
150 : : double dist_sq;
151 : : int i;
152 : : double p12[3];
153 [ # # ]: 0 : for (i = 0; i < 3; i++)
154 : 0 : p12[i] = pt2[i] - pt1[i];
155 : :
156 [ # # ]: 0 : t = parametric_position(node, pt1, pt2);
157 [ # # ]: 0 : if ( t == CUBIT_DBL_MAX )
158 : 0 : return -1.;
159 : : //if ( t== 0.0 || t == 1.0)
160 : : // return 0.0;
161 : : // is t on vector p12 or its infinite extension
162 [ # # ][ # # ]: 0 : if (t < 0.0 || t > 1.0)
163 : 0 : return -1.0;
164 : : // calculate point on p12
165 : : double p[3];
166 [ # # ]: 0 : for (i = 0; i < 3; i++)
167 : 0 : p[i] = pt1[i] + t * p12[i];
168 : :
169 : : // return distance from node to point on p12
170 : 0 : dist_sq = 0.0;
171 [ # # ]: 0 : for (i = 0; i < 3; i++)
172 : 0 : dist_sq += (p[i] - node[i]) * (p[i] - node[i]);
173 : :
174 : 0 : return sqrt(dist_sq);
175 : : }
176 : :
177 : 0 : CubitBoolean IntersectionTool::ray_tri_test(const double start[3],
178 : : const double dir[3],
179 : : const double vert0[3],
180 : : const double vert1[3],
181 : : const double vert2[3],
182 : : double &t, double &u, double &v)
183 : : {
184 : : int i;
185 : :
186 : : // find vectors for two edges sharing vert0
187 : : double edge1[3];
188 : : double edge2[3];
189 [ # # ]: 0 : for (i = 0; i < 3; i++) {
190 : 0 : edge1[i] = vert1[i] - vert0[i];
191 : 0 : edge2[i] = vert2[i] - vert0[i];
192 : : }
193 : :
194 : : // calculate determinate
195 : : double pvec[3];
196 : 0 : pvec[0] = dir[1] * edge2[2] - dir[2] * edge2[1];
197 : 0 : pvec[1] = dir[2] * edge2[0] - dir[0] * edge2[2];
198 : 0 : pvec[2] = dir[0] * edge2[1] - dir[1] * edge2[0];
199 : : double det =
200 : 0 : edge1[0] * pvec[0] + edge1[1] * pvec[1] + edge1[2] * pvec[2];
201 : :
202 : : // if determinate is near zero, the ray is in plane of triangle
203 [ # # ][ # # ]: 0 : if (det > -mTolerance && det < mTolerance) {
204 : 0 : return CUBIT_FALSE;
205 : : }
206 : :
207 : : // calculate distance from vert0 to ray origin
208 : : double tvec[3];
209 [ # # ]: 0 : for (i = 0; i < 3; i++)
210 : 0 : tvec[i] = start[i] - vert0[i];
211 : :
212 : : // calculate U parameter and test bounds
213 : 0 : double inv_det = 1.0/det;
214 : 0 : u = (tvec[0] * pvec[0] + tvec[1] * pvec[1] + tvec[2] * pvec[2]) * inv_det;
215 [ # # ][ # # ]: 0 : if (u > -mTolerance && u < mTolerance) u = 0.0;
216 [ # # ][ # # ]: 0 : else if ((u - 1.0) > -mTolerance &&
217 : 0 : (u - 1.0) < mTolerance) u = 1.0;
218 [ # # ][ # # ]: 0 : if (u < 0.0 || u > 1.0) {
219 : 0 : return CUBIT_FALSE;
220 : : }
221 : :
222 : : // calculate V parameter and test bounds
223 : : double qvec[3];
224 : 0 : qvec[0] = tvec[1] * edge1[2] - tvec[2] * edge1[1];
225 : 0 : qvec[1] = tvec[2] * edge1[0] - tvec[0] * edge1[2];
226 : 0 : qvec[2] = tvec[0] * edge1[1] - tvec[1] * edge1[0];
227 : 0 : v = (dir[0] * qvec[0] + dir[1] * qvec[1] + dir[2] * qvec[2]) * inv_det;
228 [ # # ][ # # ]: 0 : if (v > -mTolerance && v < mTolerance) v = 0.0;
229 [ # # ][ # # ]: 0 : else if ((v - 1.0) > -mTolerance &&
230 : 0 : (v - 1.0) < mTolerance) v = 1.0;
231 [ # # ][ # # ]: 0 : if (v < 0.0 || (u + v - 1.0) > mTolerance) {
232 : 0 : return CUBIT_FALSE;
233 : : }
234 : :
235 : : // calculate T, ray intersects triangle
236 : 0 : t = (edge2[0] * qvec[0] + edge2[1] * qvec[1] +
237 : 0 : edge2[2] * qvec[2]) * inv_det;
238 [ # # ][ # # ]: 0 : if (t > -mTolerance && t < mTolerance) t = 0.0;
239 [ # # ][ # # ]: 0 : else if ((t - 1.0) > -mTolerance &&
240 : 0 : (t - 1.0) < mTolerance) t = 1.0;
241 : :
242 : 0 : return CUBIT_TRUE;
243 : : }
244 : :
245 : :
246 : 0 : CubitBoolean IntersectionTool::skew_line_test(const double start1[3],
247 : : const double end1[3],
248 : : const double start2[3],
249 : : const double end2[3],
250 : : double &t, double &u)
251 : : {
252 : 0 : t = -1.0;
253 : 0 : u = -1.0;
254 : :
255 : : int i;
256 : : double p13[3], p43[3];
257 [ # # ]: 0 : for (i = 0; i < 3; i++) {
258 : 0 : p13[i] = start1[i] - start2[i];
259 : 0 : p43[i] = end2[i] - start2[i];
260 : : }
261 : 0 : double len_sq1 = p43[0] * p43[0] + p43[1] * p43[1] + p43[2] * p43[2];
262 [ # # ]: 0 : if (len_sq1 < mToleranceSquared)
263 : 0 : return CUBIT_FALSE;
264 : :
265 : : double p21[3];
266 [ # # ]: 0 : for (i = 0; i < 3; i++) {
267 : 0 : p21[i] = end1[i] - start1[i];
268 : : }
269 : 0 : double len_sq2 = p21[0] * p21[0] + p21[1] * p21[1] + p21[2] * p21[2];
270 [ # # ]: 0 : if (len_sq2 < mToleranceSquared)
271 : 0 : return CUBIT_FALSE;
272 : :
273 : : double fact;
274 [ # # ]: 0 : if (len_sq2 < len_sq1) fact = 10.0/sqrt(len_sq1);
275 : 0 : else fact = 10.0/sqrt(len_sq2);
276 [ # # ]: 0 : for (i = 0; i < 3; i++) {
277 : 0 : p13[i] *= fact;
278 : 0 : p43[i] *= fact;
279 : 0 : p21[i] *= fact;
280 : : }
281 : :
282 : : double d1343, d4321, d1321, d4343, d2121;
283 : 0 : d1343 = p13[0] * p43[0] + p13[1] * p43[1] + p13[2] * p43[2];
284 : 0 : d4321 = p43[0] * p21[0] + p43[1] * p21[1] + p43[2] * p21[2];
285 : 0 : d1321 = p13[0] * p21[0] + p13[1] * p21[1] + p13[2] * p21[2];
286 : 0 : d4343 = p43[0] * p43[0] + p43[1] * p43[1] + p43[2] * p43[2];
287 : 0 : d2121 = p21[0] * p21[0] + p21[1] * p21[1] + p21[2] * p21[2];
288 : :
289 : 0 : double denom = d2121 * d4343 - d4321 * d4321;
290 [ # # ][ # # ]: 0 : if (denom > -mTolerance && denom < mTolerance)
291 : 0 : return CUBIT_FALSE;
292 : 0 : double numer = d1343 * d4321 - d1321 * d4343;
293 : :
294 : 0 : t = numer / denom;
295 [ # # ][ # # ]: 0 : if (t > -mTolerance && t < mTolerance) t = 0.0;
296 [ # # ][ # # ]: 0 : else if ((t - 1.0) > -mTolerance &&
297 : 0 : (t - 1.0) < mTolerance) t = 1.0;
298 [ # # ][ # # ]: 0 : if (t < 0.0 || t > 1.0)
299 : 0 : return CUBIT_FALSE;
300 : :
301 : 0 : u = (d1343 + d4321 * t) / d4343;
302 [ # # ][ # # ]: 0 : if (u > -mTolerance && u < mTolerance) u = 0.0;
303 [ # # ][ # # ]: 0 : else if ((u - 1.0) > -mTolerance &&
304 : 0 : (u - 1.0) < mTolerance) u = 1.0;
305 [ # # ][ # # ]: 0 : if (u < 0.0 || u > 1.0)
306 : 0 : return CUBIT_FALSE;
307 : :
308 : 0 : return CUBIT_TRUE;
309 : : }
310 : :
311 : 0 : CubitStatus IntersectionTool::closest_points_on_segments( CubitVector &p0,
312 : : CubitVector &p1,
313 : : CubitVector &p2,
314 : : CubitVector &p3,
315 : : CubitVector &point_1,
316 : : CubitVector &point_2,
317 : : double &sc, double &tc)
318 : : {
319 [ # # ]: 0 : CubitVector u = p1 - p0;
320 [ # # ]: 0 : CubitVector v = p3 - p2;
321 [ # # ]: 0 : CubitVector w = p0 - p2;
322 [ # # ]: 0 : double a = u%u; //|u| always >= 0
323 [ # # ]: 0 : double b = u%v;
324 [ # # ]: 0 : double c = v%v; //|v| always >= 0
325 [ # # ]: 0 : double d = u%w;
326 [ # # ]: 0 : double e = v%w;
327 : 0 : double D = a*c - b*b; // always >= 0
328 : 0 : double sN, sD = D; // sc = sN / sD, default sD = D >= 0
329 : 0 : double tN, tD = D; // tc = tN / tD, default tD = D >= 0
330 : :
331 : : // compute the line parameters of the two closest points
332 [ # # ]: 0 : if (D < GEOMETRY_RESABS) { // the lines are almost parallel
333 : 0 : sN = 0.0;
334 : 0 : tN = e;
335 : 0 : tD = c;
336 : : }
337 : : else { // get the closest points on the infinite lines
338 : 0 : sN = (b*e - c*d);
339 : 0 : tN = (a*e - b*d);
340 [ # # ]: 0 : if (sN < 0) { // sc < 0 => the s=0 edge is visible
341 : 0 : sN = 0.0;
342 : 0 : tN = e;
343 : 0 : tD = c;
344 : : }
345 [ # # ]: 0 : else if (sN > sD) { // sc > 1 => the s=1 edge is visible
346 : 0 : sN = sD;
347 : 0 : tN = e + b;
348 : 0 : tD = c;
349 : : }
350 : : }
351 : :
352 [ # # ]: 0 : if (tN < 0) { // tc < 0 => the t=0 edge is visible
353 : 0 : tN = 0.0;
354 : : // recompute sc for this edge
355 [ # # ]: 0 : if (-d < 0)
356 : 0 : sN = 0.0;
357 [ # # ]: 0 : else if (-d > a)
358 : 0 : sN = sD;
359 : : else {
360 : 0 : sN = -d;
361 : 0 : sD = a;
362 : : }
363 : : }
364 [ # # ]: 0 : else if (tN > tD) { // tc > 1 => the t=1 edge is visible
365 : 0 : tN = tD;
366 : : // recompute sc for this edge
367 [ # # ]: 0 : if ((-d + b) < 0)
368 : 0 : sN = 0;
369 [ # # ]: 0 : else if ((-d + b) > a)
370 : 0 : sN = sD;
371 : : else {
372 : 0 : sN = (-d + b);
373 : 0 : sD = a;
374 : : }
375 : : }
376 : 0 : sc = CUBIT_DBL_MAX;
377 : 0 : tc = CUBIT_DBL_MAX;
378 : : //If these are going to be zero then do it...
379 [ # # ][ # # ]: 0 : if ( sN < CUBIT_RESABS && sN > -CUBIT_RESABS )
380 : 0 : sc = 0.0;
381 [ # # ][ # # ]: 0 : if ( tN < CUBIT_RESABS && tN > -CUBIT_RESABS )
382 : 0 : tc = 0.0;
383 : :
384 : : // finally do the division to get sc and tc
385 [ # # ][ # # ]: 0 : if ( sD < CUBIT_RESABS && sD > -CUBIT_RESABS && sc != 0.0 )
[ # # ]
386 : : {
387 [ # # ][ # # ]: 0 : PRINT_ERROR("About to divide by zero in closest_points_on_segments.\n");
[ # # ][ # # ]
388 : 0 : return CUBIT_FAILURE;
389 : : }
390 [ # # ][ # # ]: 0 : if ( tD < CUBIT_RESABS && tD > -CUBIT_RESABS && tc != 0.0 )
[ # # ]
391 : : {
392 [ # # ][ # # ]: 0 : PRINT_ERROR("About to divide by zero in closest_points_on_segments.\n");
[ # # ][ # # ]
393 : 0 : return CUBIT_FAILURE;
394 : : }
395 [ # # ]: 0 : if ( sc != 0.0 )
396 : 0 : sc = sN / sD;
397 [ # # ]: 0 : if ( tc != 0.0 )
398 : 0 : tc = tN / tD;
399 : :
400 [ # # ][ # # ]: 0 : point_1 = p0 + sc*u;
[ # # ]
401 [ # # ][ # # ]: 0 : point_2 = p2 + tc*v;
[ # # ]
402 : 0 : return CUBIT_SUCCESS;
403 : : }
404 : :
405 : 0 : int IntersectionTool::intersect_triangle_with_ray( CubitVector &ray_origin, CubitVector &ray_direction,
406 : : const CubitVector *p0, const CubitVector *p1, const CubitVector *p2,
407 : : CubitVector* point, double &distance, int &edge_hit )
408 : : {
409 : : // This algorithm can be found at http://geometryalgorithms.com/
410 : :
411 [ # # ]: 0 : CubitVector n; // triangle vectors
412 [ # # ][ # # ]: 0 : CubitVector w0, w; // ray vectors
413 : : double a, b; // params to calc ray-plane intersect
414 : :
415 : 0 : double tol = GEOMETRY_RESABS;
416 : :
417 : : // get triangle edge vectors and plane normal
418 [ # # ][ # # ]: 0 : CubitVector u( p1->x() - p0->x(),
419 [ # # ][ # # ]: 0 : p1->y() - p0->y(),
420 [ # # ][ # # ]: 0 : p1->z() - p0->z()); //(*p1-*p0);
[ # # ]
421 [ # # ][ # # ]: 0 : CubitVector v( p2->x() - p0->x(),
422 [ # # ][ # # ]: 0 : p2->y() - p0->y(),
423 [ # # ][ # # ]: 0 : p2->z() - p0->z()); // = (*p2-*p0);
[ # # ]
424 : :
425 [ # # ][ # # ]: 0 : n = u * v; // cross product to get normal
426 : :
427 [ # # ][ # # ]: 0 : if (n.length_squared() == 0) // triangle is degenerate
428 : 0 : return -1; // do not deal with this case
429 : :
430 : : //dir = R.P1 - R.P0; // ray direction vector
431 : : //w0 = R.P0 - T.V0;
432 [ # # ][ # # ]: 0 : w0 = CubitVector(ray_origin.x() - p0->x(),
[ # # ]
433 [ # # ][ # # ]: 0 : ray_origin.y() - p0->y(),
434 [ # # ][ # # ]: 0 : ray_origin.z() - p0->z());
[ # # ]
435 : :
436 [ # # ]: 0 : a = -(n%w0);
437 [ # # ]: 0 : b = (n%ray_direction);
438 [ # # ]: 0 : if (fabs(b) < tol) { // ray is parallel to triangle plane
439 [ # # ]: 0 : if (a == 0) // ray lies in triangle plane
440 : 0 : return 2;
441 : 0 : else return 0; // ray disjoint from plane
442 : : }
443 : :
444 : : // get intersect point of ray with triangle plane
445 : 0 : distance = a / b;
446 [ # # ]: 0 : if (distance < 0.0) // ray goes away from triangle
447 : 0 : return 0; // => no intersect
448 : : // for a segment, also test if (r > 1.0) => no intersect
449 : :
450 [ # # ][ # # ]: 0 : point->set(ray_origin + distance * ray_direction); // intersect point of ray and plane
[ # # ]
451 : :
452 : : // set distance to be absolute distance (if ray_direction was a unit vector)
453 [ # # ]: 0 : distance = distance * ray_direction.length();
454 : :
455 : : // is point inside facet?
456 : : double uu, uv, vv, wu, wv, D;
457 [ # # ]: 0 : uu = u%u;
458 [ # # ]: 0 : uv = u%v;
459 [ # # ]: 0 : vv = v%v;
460 : : //w = *I - T.V0;
461 [ # # ][ # # ]: 0 : w = CubitVector(point->x() - p0->x(),
[ # # ]
462 [ # # ][ # # ]: 0 : point->y() - p0->y(),
463 [ # # ][ # # ]: 0 : point->z() - p0->z());
[ # # ]
464 [ # # ]: 0 : wu = w%u;
465 [ # # ]: 0 : wv = w%v;
466 : 0 : D = uv * uv - uu * vv;
467 : :
468 : : // get and test parametric coords
469 : : double s, t;
470 : 0 : s = (uv * wv - vv * wu) / D;
471 [ # # ][ # # ]: 0 : if (s < 0.0 || s > 1.0) // point is outside facet
472 : 0 : return 0;
473 : 0 : t = (uv * wu - uu * wv) / D;
474 [ # # ][ # # ]: 0 : if (t < 0.0 || (s + t) > 1.0) // point is outside facet
475 : 0 : return 0;
476 : :
477 [ # # ]: 0 : if (s==0)
478 : 0 : edge_hit = 2; //lies along v, edge #2
479 [ # # ]: 0 : if (t==0)
480 : 0 : edge_hit = 1; //lies along u, edge #1
481 [ # # ]: 0 : if (s+t==1)
482 : 0 : edge_hit = 3; //lies along edge #3
483 : :
484 : : // note:
485 : : // if s and t are both 0, hit the point p0
486 : : // if s=1 and t=0, hit point p1
487 : : // if s=0 and t=1, hit point p2
488 : :
489 : 0 : return 1; // point is in facet
490 : :
491 : : }
492 : :
493 : 0 : int IntersectionTool::intersect_segment_with_ray( CubitVector &ray_origin, CubitVector &ray_direction,
494 : : const CubitVector *p0, const CubitVector *p1,
495 : : CubitVector* point, double &hit_distance, int &point_hit, double tol )
496 : : {
497 : : // This algorithm can be found at http://geometryalgorithms.com/
498 : :
499 [ # # ]: 0 : if (tol == 0.0)
500 : 0 : tol = GEOMETRY_RESABS;
501 : :
502 [ # # ]: 0 : CubitVector u = CubitVector(*p0, *p1);
503 [ # # ]: 0 : CubitVector v = ray_direction;
504 [ # # ]: 0 : v.normalize();
505 : :
506 [ # # ]: 0 : CubitVector w = CubitVector(ray_origin, *p0);
507 : :
508 : : double sc, tc; // sc is fraction along facet edge, tc is distance along ray
509 : :
510 [ # # ]: 0 : double a = u%u; // always >= 0
511 [ # # ]: 0 : double b = u%v;
512 [ # # ]: 0 : double c = v%v; // always >= 0
513 [ # # ]: 0 : double d = u%w;
514 [ # # ]: 0 : double e = v%w;
515 : 0 : double D = a*c - b*b; // always >= 0
516 : :
517 : : // compute the line parameters of the two closest points
518 [ # # ]: 0 : if (D < tol)
519 : : {
520 : : // the lines are almost parallel
521 : 0 : sc = 0.0;
522 [ # # ]: 0 : tc = (b>c ? d/b : e/c); // use the largest denominator
523 : : }
524 : : else
525 : : {
526 : 0 : sc = (b*e - c*d) / D;
527 : 0 : tc = (a*e - b*d) / D;
528 : : }
529 : :
530 : : // get the difference of the two closest points
531 [ # # ][ # # ]: 0 : CubitVector dP = CubitVector(w + (sc * u) - (tc * v)); // = <0 0 0> if intersection
[ # # ][ # # ]
532 : :
533 [ # # ]: 0 : double distance = sqrt(dP % dP); // return the closest distance (0 if intersection)
534 : :
535 [ # # ][ # # ]: 0 : point->set(*p0 + (sc * u));
[ # # ]
536 : 0 : hit_distance = tc; //distance from origin to intersection point
537 : :
538 [ # # ]: 0 : if (distance < tol)
539 : : {
540 : : //check if parallel (infinite intersection)
541 [ # # ]: 0 : if (D < tol)
542 : 0 : return 2;
543 : : //check if on edge
544 [ # # ][ # # ]: 0 : if (sc <= 1.0 && sc >= 0.0)
545 : : {
546 [ # # ]: 0 : if (sc==0)
547 : 0 : point_hit = 1; //hit point p0
548 [ # # ]: 0 : if (sc==1)
549 : 0 : point_hit = 2; //hit point p1
550 : :
551 : 0 : return 1;
552 : : }
553 : : else
554 : 0 : return 0;
555 : : }
556 : :
557 : 0 : return 0;
558 : : }
559 : :
560 : 0 : int IntersectionTool::intersect_point_with_ray( CubitVector &ray_origin, CubitVector &ray_direction,
561 : : const CubitVector* point, double &distance, double tol)
562 : : {
563 [ # # ]: 0 : if (tol == 0.0)
564 : 0 : tol = GEOMETRY_RESABS;
565 : :
566 : : //Does the ray pass through the Point?
567 : : // Calc distance from ray origin to Point.
568 : : // Then compute coord's of point along ray that distance.
569 : : // Calc distance between Point and this ray-point. If less than tolerance, a hit.
570 : :
571 [ # # ]: 0 : CubitVector pointB;
572 [ # # ]: 0 : double dist1 = point->distance_between(ray_origin);
573 [ # # ]: 0 : ray_origin.next_point(ray_direction, dist1, pointB);
574 : :
575 [ # # ][ # # ]: 0 : if ( pointB.distance_between_squared(*point) <= (tol*tol) )
576 : : {
577 : 0 : distance = dist1;
578 : 0 : return 1;
579 : : }
580 : :
581 : 0 : return 0;
582 : : }
|
