Line data Source code
1 : /* Copyright (C) Graham Rhodes, 2001.
2 : * All rights reserved worldwide.
3 : *
4 : * This software is provided "as is" without express or implied
5 : * warranties. You may freely copy and compile this source into
6 : * applications you distribute provided that the copyright text
7 : * below is included in the resulting source code, for example:
8 : * "Portions Copyright (C) Graham Rhodes, 2001"
9 : */
10 : /**************************************************************************************
11 : |
12 : | File: lineintersect_utils.cpp
13 : |
14 : | Purpose: Implementation of line segment intersection utility functions
15 : |
16 : | Book Title: Game Programming Gems II
17 : |
18 : | Chapter Title: Fast, Robust Intersection of 3D Line Segments
19 : |
20 : | Author: Graham Rhodes
21 : |
22 : | Revisions: 05-Apr-2001 - GSR. Original.
23 : |
24 : **************************************************************************************/
25 : #include <math.h>
26 : #include "lineintersect_utils.h"
27 : #include <assert.h>
28 :
29 : // uncomment the following line to have the code check intermediate results
30 : //#define CHECK_ANSWERS
31 :
32 : // uncomment the following line to use Cramer's rule instead of Gaussian elimination
33 : //#define USE_CRAMERS_RULE
34 :
35 : #define FMAX(a,b) ((a) > (b) ? (a) : (b))
36 : #define FMIN(a,b) ((a) > (b) ? (b) : (a))
37 : #define FABS(a) ((a) < 0.0f ? -(a) : (a))
38 : #define OUT_OF_RANGE(a) ((a) < 0.0f || (a) > 1.f)
39 :
40 : // pragma to get rid of math.h inline function removal warnings.
41 : //#pragma warning(disable:4514)
42 :
43 : /**************************************************************************
44 : |
45 : | Method: IntersectLineSegments
46 : |
47 : | Purpose: Find the nearest point between two finite length line segments
48 : | or two infinite lines in 3-dimensional space. The function calculates
49 : | the point on each line/line segment that is closest to the other
50 : | line/line segment, the midpoint between the nearest points, and
51 : | the vector between these two points. If the two nearest points
52 : | are close within a tolerance, a flag is set indicating the lines
53 : | have a "true" intersection.
54 : |
55 : | Parameters: Input:
56 : | ------
57 : | A1x, A1y, A1z - Coordinates of first defining point of line/segment A
58 : | A2x, A2y, A2z - Coordinates of second defining point of line/segment A
59 : | B1x, B1y, B1z - Coordinates of first defining point of line/segment B
60 : | B2x, B2y, B2z - Coordinates of second defining point of line/segment B
61 : | infinite_lines - set to true if lines are to be treated as infinite
62 : | epsilon - tolerance value to be used to check for degenerate
63 : | and parallel lines, and to check for true intersection.
64 : |
65 : | Output:
66 : | -------
67 : | PointOnSegAx, - Coordinates of the point on segment A that are nearest
68 : | PointOnSegAy, to segment B. This corresponds to point C in the text.
69 : | PointOnSegAz
70 : | PointOnSegBx, - Coordinates of the point on segment B that are nearest
71 : | PointOnSegBy, to segment A. This corresponds to point D in the text.
72 : | PointOnSegBz
73 : | NearestPointX, - Midpoint between the two nearest points. This can be
74 : | NearestPointY, treated as *the* intersection point if nearest points
75 : | NearestPointZ are sufficiently close. This corresponds to point P
76 : | in the text.
77 : | NearestVectorX, - Vector between the nearest point on A to the nearest
78 : | point on segment B. This vector is normal to both
79 : | lines if the lines are infinite, but is not guaranteed
80 : | to be normal to both lines if both lines are finite
81 : | length.
82 : | true_intersection - true if the nearest points are close within a small
83 : | tolerance.
84 : **************************************************************************/
85 0 : void IntersectLineSegments(const number A1x, const number A1y, const number A1z,
86 : const number A2x, const number A2y, const number A2z,
87 : const number B1x, const number B1y, const number B1z,
88 : const number B2x, const number B2y, const number B2z,
89 : bool infinite_lines, number epsilon, number &PointOnSegAx,
90 : number &PointOnSegAy, number &PointOnSegAz, number &PointOnSegBx,
91 : number &PointOnSegBy, number &PointOnSegBz, number &NearestPointX,
92 : number &NearestPointY, number &NearestPointZ, number &NearestVectorX,
93 : number &NearestVectorY, number &NearestVectorZ, bool &true_intersection)
94 : {
95 0 : number temp = 0.f;
96 0 : number epsilon_squared = epsilon * epsilon;
97 :
98 : // Compute parameters from Equations (1) and (2) in the text
99 0 : number Lax = A2x - A1x;
100 0 : number Lay = A2y - A1y;
101 0 : number Laz = A2z - A1z;
102 0 : number Lbx = B2x - B1x;
103 0 : number Lby = B2y - B1y;
104 0 : number Lbz = B2z - B1z;
105 : // From Equation (15)
106 0 : number L11 = (Lax * Lax) + (Lay * Lay) + (Laz * Laz);
107 0 : number L22 = (Lbx * Lbx) + (Lby * Lby) + (Lbz * Lbz);
108 :
109 : // Line/Segment A is degenerate ---- Special Case #1
110 0 : if (L11 < epsilon_squared)
111 : {
112 0 : PointOnSegAx = A1x;
113 0 : PointOnSegAy = A1y;
114 0 : PointOnSegAz = A1z;
115 0 : FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz, A1x, A1y, A1z,
116 : infinite_lines, epsilon, PointOnSegBx, PointOnSegBy,
117 : PointOnSegBz, temp);
118 : }
119 : // Line/Segment B is degenerate ---- Special Case #1
120 0 : else if (L22 < epsilon_squared)
121 : {
122 0 : PointOnSegBx = B1x;
123 0 : PointOnSegBy = B1y;
124 0 : PointOnSegBz = B1z;
125 0 : FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, B1x, B1y, B1z,
126 : infinite_lines, epsilon, PointOnSegAx, PointOnSegAy,
127 : PointOnSegAz, temp);
128 : }
129 : // Neither line/segment is degenerate
130 : else
131 : {
132 : // Compute more parameters from Equation (3) in the text.
133 0 : number ABx = B1x - A1x;
134 0 : number ABy = B1y - A1y;
135 0 : number ABz = B1z - A1z;
136 :
137 : // and from Equation (15).
138 0 : number L12 = -(Lax * Lbx) - (Lay * Lby) - (Laz * Lbz);
139 :
140 0 : number DetL = L11 * L22 - L12 * L12;
141 : // Lines/Segments A and B are parallel ---- special case #2.
142 0 : if (FABS(DetL) < epsilon)
143 : {
144 0 : FindNearestPointOfParallelLineSegments(A1x, A1y, A1z, A2x, A2y, A2z,
145 : Lax, Lay, Laz,
146 : B1x, B1y, B1z, B2x, B2y, B2z,
147 : Lbx, Lby, Lbz,
148 : infinite_lines, epsilon,
149 : PointOnSegAx, PointOnSegAy, PointOnSegAz,
150 : PointOnSegBx, PointOnSegBy, PointOnSegBz);
151 : }
152 : // The general case
153 : else
154 : {
155 : // from Equation (15)
156 0 : number ra = Lax * ABx + Lay * ABy + Laz * ABz;
157 0 : number rb = -Lbx * ABx - Lby * ABy - Lbz * ABz;
158 :
159 0 : number t = (L11 * rb - ra * L12)/DetL; // Equation (12)
160 :
161 : #ifdef USE_CRAMERS_RULE
162 : number s = (L22 * ra - rb * L12)/DetL;
163 : #else
164 0 : number s = (ra-L12*t)/L11; // Equation (13)
165 : #endif // USE_CRAMERS_RULE
166 :
167 : #ifdef CHECK_ANSWERS
168 : number check_ra = s*L11 + t*L12;
169 : number check_rb = s*L12 + t*L22;
170 : assert(FABS(check_ra-ra) < epsilon);
171 : assert(FABS(check_rb-rb) < epsilon);
172 : #endif // CHECK_ANSWERS
173 :
174 : // if we are dealing with infinite lines or if parameters s and t both
175 : // lie in the range [0,1] then just compute the points using Equations
176 : // (1) and (2) from the text.
177 0 : PointOnSegAx = (A1x + s * Lax);
178 0 : PointOnSegAy = (A1y + s * Lay);
179 0 : PointOnSegAz = (A1z + s * Laz);
180 0 : PointOnSegBx = (B1x + t * Lbx);
181 0 : PointOnSegBy = (B1y + t * Lby);
182 0 : PointOnSegBz = (B1z + t * Lbz);
183 : // otherwise, at least one of s and t is outside of [0,1] and we have to
184 : // handle this case.
185 0 : if (false == infinite_lines && (OUT_OF_RANGE(s) || OUT_OF_RANGE(t)))
186 : {
187 0 : AdjustNearestPoints(A1x, A1y, A1z, Lax, Lay, Laz,
188 : B1x, B1y, B1z, Lbx, Lby, Lbz,
189 : epsilon, s, t,
190 : PointOnSegAx, PointOnSegAy, PointOnSegAz,
191 : PointOnSegBx, PointOnSegBy, PointOnSegBz);
192 : }
193 : }
194 : }
195 :
196 0 : NearestPointX = 0.5f * (PointOnSegAx + PointOnSegBx);
197 0 : NearestPointY = 0.5f * (PointOnSegAy + PointOnSegBy);
198 0 : NearestPointZ = 0.5f * (PointOnSegAz + PointOnSegBz);
199 :
200 0 : NearestVectorX = PointOnSegBx - PointOnSegAx;
201 0 : NearestVectorY = PointOnSegBy - PointOnSegAy;
202 0 : NearestVectorZ = PointOnSegBz - PointOnSegAz;
203 :
204 : // optional check to indicate if the lines truly intersect
205 0 : true_intersection = (FABS(NearestVectorX) +
206 0 : FABS(NearestVectorY) +
207 0 : FABS(NearestVectorZ)) < epsilon ? true : false;
208 0 : }
209 :
210 : /**************************************************************************
211 : |
212 : | Method: FindNearestPointOnLineSegment
213 : |
214 : | Purpose: Given a line (segment) and a point in 3-dimensional space,
215 : | find the point on the line (segment) that is closest to the
216 : | point.
217 : |
218 : | Parameters: Input:
219 : | ------
220 : | A1x, A1y, A1z - Coordinates of first defining point of the line/segment
221 : | Lx, Ly, Lz - Vector from (A1x, A1y, A1z) to the second defining point
222 : | of the line/segment.
223 : | Bx, By, Bz - Coordinates of the point
224 : | infinite_lines - set to true if lines are to be treated as infinite
225 : | epsilon_squared - tolerance value to be used to check for degenerate
226 : | and parallel lines, and to check for true intersection.
227 : |
228 : | Output:
229 : | -------
230 : | NearestPointX, - Point on line/segment that is closest to (Bx, By, Bz)
231 : | NearestPointY,
232 : | NearestPointZ
233 : | parameter - Parametric coordinate of the nearest point along the
234 : | line/segment. parameter = 0 at (A1x, A1y, A1z) and
235 : | parameter = 1 at the second defining point of the line/
236 : | segmetn
237 : **************************************************************************/
238 0 : void FindNearestPointOnLineSegment(const number A1x, const number A1y, const number A1z,
239 : const number Lx, const number Ly, const number Lz,
240 : const number Bx, const number By, const number Bz,
241 : bool infinite_line, number epsilon_squared, number &NearestPointX,
242 : number &NearestPointY, number &NearestPointZ,
243 : number ¶meter)
244 : {
245 : // Line/Segment is degenerate --- special case #1
246 0 : number D = Lx * Lx + Ly * Ly + Lz * Lz;
247 0 : if (D < epsilon_squared)
248 : {
249 0 : NearestPointX = A1x;
250 0 : NearestPointY = A1y;
251 0 : NearestPointZ = A1z;
252 0 : return;
253 : }
254 :
255 0 : number ABx = Bx - A1x;
256 0 : number ABy = By - A1y;
257 0 : number ABz = Bz - A1z;
258 :
259 : // parameter is computed from Equation (20).
260 0 : parameter = (Lx * ABx + Ly * ABy + Lz * ABz) / D;
261 :
262 0 : if (false == infinite_line) parameter = FMAX(0.0f, FMIN(1.0f, parameter));
263 :
264 0 : NearestPointX = A1x + parameter * Lx;
265 0 : NearestPointY = A1y + parameter * Ly;
266 0 : NearestPointZ = A1z + parameter * Lz;
267 0 : return;
268 : }
269 :
270 : /**************************************************************************
271 : |
272 : | Method: FindNearestPointOfParallelLineSegments
273 : |
274 : | Purpose: Given two lines (segments) that are known to be parallel, find
275 : | a representative point on each that is nearest to the other. If
276 : | the lines are considered to be finite then it is possible that there
277 : | is one true point on each line that is nearest to the other. This
278 : | code properly handles this case.
279 : |
280 : | This is the most difficult line intersection case to handle, since
281 : | there is potentially a family, or locus of points on each line/segment
282 : | that are nearest to the other.
283 : | Parameters: Input:
284 : | ------
285 : | A1x, A1y, A1z - Coordinates of first defining point of line/segment A
286 : | A2x, A2y, A2z - Coordinates of second defining point of line/segment A
287 : | Lax, Lay, Laz - Vector from (A1x, A1y, A1z) to the (A2x, A2y, A2z).
288 : | B1x, B1y, B1z - Coordinates of first defining point of line/segment B
289 : | B2x, B2y, B2z - Coordinates of second defining point of line/segment B
290 : | Lbx, Lby, Lbz - Vector from (B1x, B1y, B1z) to the (B2x, B2y, B2z).
291 : | infinite_lines - set to true if lines are to be treated as infinite
292 : | epsilon_squared - tolerance value to be used to check for degenerate
293 : | and parallel lines, and to check for true intersection.
294 : |
295 : | Output:
296 : | -------
297 : | PointOnSegAx, - Coordinates of the point on segment A that are nearest
298 : | PointOnSegAy, to segment B. This corresponds to point C in the text.
299 : | PointOnSegAz
300 : | PointOnSegBx, - Coordinates of the point on segment B that are nearest
301 : | PointOnSegBy, to segment A. This corresponds to point D in the text.
302 : | PointOnSegBz
303 :
304 : **************************************************************************/
305 0 : void FindNearestPointOfParallelLineSegments(number A1x, number A1y, number A1z,
306 : number A2x, number A2y, number A2z,
307 : number Lax, number Lay, number Laz,
308 : number B1x, number B1y, number B1z,
309 : number B2x, number B2y, number B2z,
310 : number Lbx, number Lby, number Lbz,
311 : bool infinite_lines, number epsilon_squared,
312 : number &PointOnSegAx, number &PointOnSegAy, number &PointOnSegAz,
313 : number &PointOnSegBx, number &PointOnSegBy, number &PointOnSegBz)
314 : {
315 0 : number s[2] = {0, 0};
316 : number temp;
317 0 : FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, B1x, B1y, B1z,
318 : true, epsilon_squared, PointOnSegAx, PointOnSegAy, PointOnSegAz, s[0]);
319 0 : if (true == infinite_lines)
320 : {
321 0 : PointOnSegBx = B1x;
322 0 : PointOnSegBy = B1y;
323 0 : PointOnSegBz = B1z;
324 : }
325 : else
326 : {
327 : number tp[3];
328 0 : FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, B2x, B2y, B2z,
329 : true, epsilon_squared, tp[0], tp[1], tp[2], s[1]);
330 0 : if (s[0] < 0.f && s[1] < 0.f)
331 : {
332 0 : PointOnSegAx = A1x;
333 0 : PointOnSegAy = A1y;
334 0 : PointOnSegAz = A1z;
335 0 : if (s[0] < s[1])
336 : {
337 0 : PointOnSegBx = B2x;
338 0 : PointOnSegBy = B2y;
339 0 : PointOnSegBz = B2z;
340 : }
341 : else
342 : {
343 0 : PointOnSegBx = B1x;
344 0 : PointOnSegBy = B1y;
345 0 : PointOnSegBz = B1z;
346 : }
347 : }
348 0 : else if (s[0] > 1.f && s[1] > 1.f)
349 : {
350 0 : PointOnSegAx = A2x;
351 0 : PointOnSegAy = A2y;
352 0 : PointOnSegAz = A2z;
353 0 : if (s[0] < s[1])
354 : {
355 0 : PointOnSegBx = B1x;
356 0 : PointOnSegBy = B1y;
357 0 : PointOnSegBz = B1z;
358 : }
359 : else
360 : {
361 0 : PointOnSegBx = B2x;
362 0 : PointOnSegBy = B2y;
363 0 : PointOnSegBz = B2z;
364 : }
365 : }
366 : else
367 : {
368 0 : temp = 0.5f*(FMAX(0.0f, FMIN(1.0f, s[0])) + FMAX(0.0f, FMIN(1.0f, s[1])));
369 0 : PointOnSegAx = (A1x + temp * Lax);
370 0 : PointOnSegAy = (A1y + temp * Lay);
371 0 : PointOnSegAz = (A1z + temp * Laz);
372 0 : FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz,
373 : PointOnSegAx, PointOnSegAy, PointOnSegAz, true,
374 : epsilon_squared, PointOnSegBx, PointOnSegBy, PointOnSegBz, temp);
375 : }
376 : }
377 0 : }
378 :
379 : /**************************************************************************
380 : |
381 : | Method: AdjustNearestPoints
382 : |
383 : | Purpose: Given nearest point information for two infinite lines, adjust
384 : | to model finite line segments.
385 : |
386 : | Parameters: Input:
387 : | ------
388 : | A1x, A1y, A1z - Coordinates of first defining point of line/segment A
389 : | Lax, Lay, Laz - Vector from (A1x, A1y, A1z) to the (A2x, A2y, A2z).
390 : | B1x, B1y, B1z - Coordinates of first defining point of line/segment B
391 : | Lbx, Lby, Lbz - Vector from (B1x, B1y, B1z) to the (B2x, B2y, B2z).
392 : | epsilon_squared - tolerance value to be used to check for degenerate
393 : | and parallel lines, and to check for true intersection.
394 : | s - parameter representing nearest point on infinite line A
395 : | t - parameter representing nearest point on infinite line B
396 : |
397 : | Output:
398 : | -------
399 : | PointOnSegAx, - Coordinates of the point on segment A that are nearest
400 : | PointOnSegAy, to segment B. This corresponds to point C in the text.
401 : | PointOnSegAz
402 : | PointOnSegBx, - Coordinates of the point on segment B that are nearest
403 : | PointOnSegBy, to segment A. This corresponds to point D in the text.
404 : | PointOnSegBz
405 : **************************************************************************/
406 0 : void AdjustNearestPoints(number A1x, number A1y, number A1z,
407 : number Lax, number Lay, number Laz,
408 : number B1x, number B1y, number B1z,
409 : number Lbx, number Lby, number Lbz,
410 : number epsilon_squared, number s, number t,
411 : number &PointOnSegAx, number &PointOnSegAy, number &PointOnSegAz,
412 : number &PointOnSegBx, number &PointOnSegBy, number &PointOnSegBz)
413 : {
414 : // handle the case where both parameter s and t are out of range
415 0 : if (OUT_OF_RANGE(s) && OUT_OF_RANGE(t))
416 : {
417 0 : s = FMAX(0.0f, FMIN(1.0f, s));
418 0 : PointOnSegAx = (A1x + s * Lax);
419 0 : PointOnSegAy = (A1y + s * Lay);
420 0 : PointOnSegAz = (A1z + s * Laz);
421 0 : FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz, PointOnSegAx,
422 : PointOnSegAy, PointOnSegAz, true, epsilon_squared,
423 : PointOnSegBx, PointOnSegBy, PointOnSegBz, t);
424 0 : if (OUT_OF_RANGE(t))
425 : {
426 0 : t = FMAX(0.0f, FMIN(1.0f, t));
427 0 : PointOnSegBx = (B1x + t * Lbx);
428 0 : PointOnSegBy = (B1y + t * Lby);
429 0 : PointOnSegBz = (B1z + t * Lbz);
430 0 : FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, PointOnSegBx,
431 : PointOnSegBy, PointOnSegBz, false, epsilon_squared,
432 : PointOnSegAx, PointOnSegAy, PointOnSegAz, s);
433 0 : FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz, PointOnSegAx,
434 : PointOnSegAy, PointOnSegAz, false, epsilon_squared,
435 : PointOnSegBx, PointOnSegBy, PointOnSegBz, t);
436 : }
437 : }
438 : // otherwise, handle the case where the parameter for only one segment is
439 : // out of range
440 0 : else if (OUT_OF_RANGE(s))
441 : {
442 0 : s = FMAX(0.0f, FMIN(1.0f, s));
443 0 : PointOnSegAx = (A1x + s * Lax);
444 0 : PointOnSegAy = (A1y + s * Lay);
445 0 : PointOnSegAz = (A1z + s * Laz);
446 0 : FindNearestPointOnLineSegment(B1x, B1y, B1z, Lbx, Lby, Lbz, PointOnSegAx,
447 : PointOnSegAy, PointOnSegAz, false, epsilon_squared,
448 : PointOnSegBx, PointOnSegBy, PointOnSegBz, t);
449 : }
450 0 : else if (OUT_OF_RANGE(t))
451 : {
452 0 : t = FMAX(0.0f, FMIN(1.0f, t));
453 0 : PointOnSegBx = (B1x + t * Lbx);
454 0 : PointOnSegBy = (B1y + t * Lby);
455 0 : PointOnSegBz = (B1z + t * Lbz);
456 0 : FindNearestPointOnLineSegment(A1x, A1y, A1z, Lax, Lay, Laz, PointOnSegBx,
457 : PointOnSegBy, PointOnSegBz, false, epsilon_squared,
458 : PointOnSegAx, PointOnSegAy, PointOnSegAz, s);
459 : }
460 : else
461 : {
462 : assert(0);
463 : }
464 0 : }
|
