Debian Codegraph @

lib/database/src/sb/SbLine.c++

raw
1 /*
2 *
3 * Copyright (C) 2000 Silicon Graphics, Inc. All Rights Reserved.
4 *
5 * This library is free software; you can redistribute it and/or
6 * modify it under the terms of the GNU Lesser General Public
7 * License as published by the Free Software Foundation; either
8 * version 2.1 of the License, or (at your option) any later version.
9 *
10 * This library is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * Lesser General Public License for more details.
14 *
15 * Further, this software is distributed without any warranty that it is
16 * free of the rightful claim of any third person regarding infringement
17 * or the like. Any license provided herein, whether implied or
18 * otherwise, applies only to this software file. Patent licenses, if
19 * any, provided herein do not apply to combinations of this program with
20 * other software, or any other product whatsoever.
21 *
22 * You should have received a copy of the GNU Lesser General Public
23 * License along with this library; if not, write to the Free Software
24 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
25 *
26 * Contact information: Silicon Graphics, Inc., 1600 Amphitheatre Pkwy,
27 * Mountain View, CA 94043, or:
28 *
29 * http://www.sgi.com
30 *
31 * For further information regarding this notice, see:
32 *
33 * http://oss.sgi.com/projects/GenInfo/NoticeExplan/
34 *
35 */
36
37 /*
38 * Copyright (C) 1990,91 Silicon Graphics, Inc.
39 *
40 _______________________________________________________________________
41 ______________ S I L I C O N G R A P H I C S I N C . ____________
42 |
43 | $Revision: 1.1.1.1 $
44 |
45 | Classes:
46 | SbLine
47 |
48 | Author(s) : Nick Thompson, Howard Look
49 |
50 ______________ S I L I C O N G R A P H I C S I N C . ____________
51 _______________________________________________________________________
52 */
53
54 #include <Inventor/SbBox.h>
55 #include <Inventor/SbLinear.h>
56
57 //////////////////////////////////////////////////////////////////////////////
58 //
59 // Line class
60 //
61 //////////////////////////////////////////////////////////////////////////////
62
63 // construct a line given 2 points on the line
64 SbLine::SbLine(const SbVec3f &p0, const SbVec3f &p1)
65 {
66 setValue(p0, p1);
67 }
68
69 // setValue constructs a line give two points on the line
70 void
71 SbLine::setValue(const SbVec3f &p0, const SbVec3f &p1)
72 {
73 pos = p0;
74 dir = p1 - p0;
75 dir.normalize();
76 }
77
78 // find points on this line and on line2 that are closest to each other.
79 // If the lines intersect, then ptOnThis and ptOnLine2 will be equal.
80 // If the lines are parallel, then FALSE is returned, and the contents of
81 // ptOnThis and ptOnLine2 are undefined.
82 SbBool
83 SbLine::getClosestPoints( const SbLine &line2,
84 SbVec3f &ptOnThis,
85 SbVec3f &ptOnLine2 ) const
86 {
87 float s, t, A, B, C, D, E, F, denom;
88 SbVec3f pos2 = line2.getPosition();
89 SbVec3f dir2 = line2.getDirection();
90
91
92 // DERIVATION:
93 // [1] parametric descriptions of desired pts
94 // pos + s * dir = ptOnThis
95 // pos2 + t * dir2 = ptOnLine2
96 // [2] (By some theorem or other--)
97 // If these are closest points between lines, then line connecting
98 // these points is perpendicular to both this line and line2.
99 // dir . ( ptOnLine2 - ptOnThis ) = 0
100 // dir2 . ( ptOnLine2 - ptOnThis ) = 0
101 // OR...
102 // dir . ( pos2 + t * dir2 - pos - s * dir ) = 0
103 // dir2 . ( pos2 + t * dir2 - pos - s * dir ) = 0
104 // [3] Rearrange the terms:
105 // t * [ dir . dir2 ] - s * [dir . dir ] = dir . pos - dir . pos2
106 // t * [ dir2 . dir2 ] - s * [dir2 . dir ] = dir2 . pos - dir2 . pos2
107 // [4] Let:
108 // A= dir . dir2
109 // B= dir . dir
110 // C= dir . pos - dir . pos2
111 // D= dir2 . dir2
112 // E= dir2 . dir
113 // F= dir2 . pos - dir2 . pos2
114 // So [3] above turns into:
115 // t * A - s * B = C
116 // t * D - s * E = F
117 // [5] Solving for s gives:
118 // s = (CD - AF) / (AE - BD)
119 // [6] Solving for t gives:
120 // t = (CE - BF) / (AE - BD)
121
122 A = dir.dot( dir2 );
123 B = dir.dot( dir );
124 C = dir.dot( pos ) - dir.dot( pos2 );
125 D = dir2.dot( dir2 );
126 E = dir2.dot( dir );
127 F = dir2.dot( pos ) - dir2.dot( pos2 );
128
129
130 denom = A * E - B * D;
131 if ( denom == 0.0) // the two lines are parallel
132 return FALSE;
133
134 s = ( C * D - A * F ) / denom;
135 t = ( C * E - B * F ) / denom;
136 ptOnThis = pos + dir * s;
137 ptOnLine2 = pos2 + dir2 * t;
138
139 return TRUE;
140 }
141
142
143 ////////////////////////////////////////////////////////////////////////
144 //
145 // Description:
146 // Returns the closes point on this line to the given point.
147 //
148 // Use: public
149
150 SbVec3f
151 SbLine::getClosestPoint(const SbVec3f &point) const
152 //
153 ////////////////////////////////////////////////////////////////////////
154 {
155 // vector from origin of this line to given point
156 SbVec3f p = point - pos;
157
158 // find the length of p when projected onto this line
159 // (which has direction d)
160 // length = |p| * cos(angle b/w p and d) = (p.d)/|d|
161 // but |d| = 1, so
162 float length = p.dot(dir);
163
164 // vector coincident with this line
165 SbVec3f proj = dir;
166 proj *= length;
167
168 SbVec3f result = pos + proj;
169 return result;
170 }
171
172
173 ////////////////////////////////////////////////////////////////////////
174 //
175 // Description:
176 // Intersect the line with a 3D box. The line is intersected with the
177 // twelve triangles that make up the box.
178 //
179 // Use: internal
180
181 SbBool
182 SbLine::intersect(const SbBox3f &box, SbVec3f &enter, SbVec3f &exit) const
183 //
184 ////////////////////////////////////////////////////////////////////////
185 {
186 if (box.isEmpty())
187 return FALSE;
188
189 const SbVec3f &pos = getPosition(), &dir = getDirection();
190 const SbVec3f &max = box.getMax(), &min = box.getMin();
191 SbVec3f points[8], inter, bary;
192 SbPlane plane;
193 int i, v0, v1, v2;
194 SbBool front = FALSE, valid, validIntersection = FALSE;
195
196 //
197 // First, check the distance from the ray to the center
198 // of the box. If that distance is greater than 1/2
199 // the diagonal distance, there is no intersection
200 // diff is the vector from the closest point on the ray to the center
201 // dist2 is the square of the distance from the ray to the center
202 // rad2 is the square of 1/2 the diagonal length of the bounding box
203 //
204 float t = (box.getCenter() - pos).dot(dir);
205 SbVec3f diff(pos + t * dir - box.getCenter());
206 float dist2 = diff.dot(diff);
207 float rad2 = (max - min).dot(max - min) * .25;
208
209 if (dist2 > rad2)
210 return FALSE;
211
212 // set up the eight coords of the corners of the box
213 for(i = 0; i < 8; i++)
214 points[i].setValue(i & 01 ? min[0] : max[0],
215 i & 02 ? min[1] : max[1],
216 i & 04 ? min[2] : max[2]);
217
218 // intersect the 12 triangles.
219 for(i = 0; i < 12; i++) {
220 switch(i) {
221 case 0: v0 = 2; v1 = 1; v2 = 0; break; // +z
222 case 1: v0 = 2; v1 = 3; v2 = 1; break;
223
224 case 2: v0 = 4; v1 = 5; v2 = 6; break; // -z
225 case 3: v0 = 6; v1 = 5; v2 = 7; break;
226
227 case 4: v0 = 0; v1 = 6; v2 = 2; break; // -x
228 case 5: v0 = 0; v1 = 4; v2 = 6; break;
229
230 case 6: v0 = 1; v1 = 3; v2 = 7; break; // +x
231 case 7: v0 = 1; v1 = 7; v2 = 5; break;
232
233 case 8: v0 = 1; v1 = 4; v2 = 0; break; // -y
234 case 9: v0 = 1; v1 = 5; v2 = 4; break;
235
236 case 10: v0 = 2; v1 = 7; v2 = 3; break; // +y
237 case 11: v0 = 2; v1 = 6; v2 = 7; break;
238 }
239 if ((valid = intersect(points[v0], points[v1], points[v2],
240 inter, bary, front)) == TRUE) {
241 if (front) {
242 enter = inter;
243 validIntersection = valid;
244 }
245 else {
246 exit = inter;
247 validIntersection = valid;
248 }
249 }
250 }
251 return validIntersection;
252 }
253
254 ////////////////////////////////////////////////////////////////////////
255 //
256 // Description:
257 // Intersect the line with a 3D box. The line is augmented with an
258 // angle to form a cone. The box must lie within pickAngle of the
259 // line. If the angle is < 0.0, abs(angle) is the radius of a
260 // cylinder for an orthographic intersection.
261 //
262 // Use: internal
263
264 SbBool
265 SbLine::intersect(float angle, const SbBox3f &box) const
266 //
267 ////////////////////////////////////////////////////////////////////////
268 {
269 if (box.isEmpty())
270 return FALSE;
271
272 const SbVec3f &max = box.getMax(), &min = box.getMin();
273 float fuzz = 0.0;
274 int i;
275
276 if (angle < 0.0)
277 fuzz = - angle;
278
279 else {
280 // Find the farthest point on the bounding box (where the pick
281 // cone will be largest). The amount of fuzz at this point will
282 // be the minimum we can use. Expand the box by that amount and
283 // do an intersection.
284 double tanA = tan(angle);
285 for(i = 0; i < 8; i++) {
286 SbVec3f point(i & 01 ? min[0] : max[0],
287 i & 02 ? min[1] : max[1],
288 i & 04 ? min[2] : max[2]);
289 // how far is point from line origin??
290 SbVec3f diff(point - getPosition());
291 double thisFuzz = sqrt(diff.dot(diff)) * tanA;
292
293 if (thisFuzz > fuzz)
294 fuzz = thisFuzz;
295 }
296 }
297
298 SbBox3f fuzzBox = box;
299
300 fuzzBox.extendBy(SbVec3f(min[0] - fuzz, min[1] - fuzz, min[2] - fuzz));
301 fuzzBox.extendBy(SbVec3f(max[0] + fuzz, max[1] + fuzz, max[2] + fuzz));
302
303 SbVec3f scratch1, scratch2;
304 return intersect(fuzzBox, scratch1, scratch2);
305 }
306
307 ////////////////////////////////////////////////////////////////////////
308 //
309 // Description:
310 // Intersect the line with a point. The line is augmented with an
311 // angle to form a cone. The point must lie within pickAngle of the
312 // line. If the angle is < 0.0, abs(angle) is
313 // the radius of a cylinder for an orthographic intersection.
314 //
315 // Use: internal
316
317 SbBool
318 SbLine::intersect(
319 float pickAngle, // The angle which makes the cone
320 const SbVec3f &point) const // The point to interesect.
321 //
322 ////////////////////////////////////////////////////////////////////////
323 {
324 float t, d;
325
326 // how far is point from line origin??
327 SbVec3f diff(point - getPosition());
328
329 t = diff.dot(getDirection());
330 if(t > 0) {
331 d = sqrt(diff.dot(diff) - t*t);
332 if (pickAngle < 0.0)
333 return (d < -pickAngle);
334 return ((d/t) < pickAngle);
335 }
336 return FALSE;
337 }
338
339 ////////////////////////////////////////////////////////////////////////
340 //
341 // Description:
342 // Intersect the line with a line segment. The line is augmented with
343 // an angle to form a cone. The line segment must lie within pickAngle
344 // of the line. If an intersection occurs, the intersection point is
345 // placed in intersection.
346 //
347 // Use: internal
348
349 SbBool
350 SbLine::intersect(
351 float pickAngle, // The angle which makes the cone
352 const SbVec3f &v0, // One endpoint of the line segment
353 const SbVec3f &v1, // The other endpoint of the line segment
354 SbVec3f &intersection) const // The intersection point
355 //
356 ////////////////////////////////////////////////////////////////////////
357 {
358 SbVec3f ptOnLine;
359 SbLine inputLine(v0, v1);
360 float distance;
361 SbBool validIntersection = FALSE;
362
363 if(getClosestPoints(inputLine, ptOnLine, intersection)) {
364 // check to make sure the intersection is within the line segment
365 if((intersection - v0).dot(v1 - v0) < 0)
366 intersection = v0;
367 else if((intersection - v1).dot(v0 - v1) < 0)
368 intersection = v1;
369
370 distance = (ptOnLine - intersection).length();
371 if (pickAngle < 0.0)
372 return (distance < -pickAngle);
373 validIntersection = ((distance / (ptOnLine - getPosition()).length())
374 < pickAngle);
375 }
376 return validIntersection;
377 }
378
379 ////////////////////////////////////////////////////////////////////////
380 //
381 // Description:
382 // Intersects the line with the triangle formed bu v0, v1, v2.
383 // Returns TRUE if there is an intersection. If there is an
384 // intersection, barycentric will contain the barycentric
385 // coordinates of the intersection (for v0, v1, v2, respectively)
386 // and front will be set to TRUE if the ray intersected the front
387 // of the triangle (as defined by the right hand rule).
388 //
389 // Use: internal
390
391 SbBool
392 SbLine::intersect(const SbVec3f &v0, const SbVec3f &v1, const SbVec3f &v2,
393 SbVec3f &intersection,
394 SbVec3f &barycentric, SbBool &front) const
395 //
396 ////////////////////////////////////////////////////////////////////////
397 {
398 //////////////////////////////////////////////////////////////////
399 //
400 // The method used here is described by Didier Badouel in Graphics
401 // Gems (I), pages 390 - 393.
402 //
403 //////////////////////////////////////////////////////////////////
404
405 #define EPSILON 1e-10
406
407 //
408 // (1) Compute the plane containing the triangle
409 //
410 SbVec3f v01 = v1 - v0;
411 SbVec3f v12 = v2 - v1;
412 SbVec3f norm = v12.cross(v01); // Un-normalized normal
413 // Normalize normal to unit length, and make sure the length is
414 // not 0 (indicating a zero-area triangle)
415 if (norm.normalize() < EPSILON)
416 return FALSE;
417
418 //
419 // (2) Compute the distance t to the plane along the line
420 //
421 float d = getDirection().dot(norm);
422 if (d < EPSILON && d > -EPSILON)
423 return FALSE; // Line is parallel to plane
424 float t = norm.dot(v0 - getPosition()) / d;
425
426 // Note: we DO NOT ignore intersections behind the eye (t < 0.0)
427
428 //
429 // (3) Find the largest component of the plane normal. The other
430 // two dimensions are the axes of the aligned plane we will
431 // use to project the triangle.
432 //
433 float xAbs = norm[0] < 0.0 ? -norm[0] : norm[0];
434 float yAbs = norm[1] < 0.0 ? -norm[1] : norm[1];
435 float zAbs = norm[2] < 0.0 ? -norm[2] : norm[2];
436 int axis0, axis1;
437
438 if (xAbs > yAbs && xAbs > zAbs) {
439 axis0 = 1;
440 axis1 = 2;
441 }
442 else if (yAbs > zAbs) {
443 axis0 = 2;
444 axis1 = 0;
445 }
446 else {
447 axis0 = 0;
448 axis1 = 1;
449 }
450
451 //
452 // (4) Determine if the projected intersection (of the line and
453 // the triangle's plane) lies within the projected triangle.
454 // Since we deal with only 2 components, we can avoid the
455 // third computation.
456 //
457 float intersection0 = getPosition()[axis0] + t * getDirection()[axis0];
458 float intersection1 = getPosition()[axis1] + t * getDirection()[axis1];
459
460 SbVec2f diff0, diff1, diff2;
461 SbBool isInter = FALSE;
462 float alpha, beta;
463
464 diff0[0] = intersection0 - v0[axis0];
465 diff0[1] = intersection1 - v0[axis1];
466 diff1[0] = v1[axis0] - v0[axis0];
467 diff1[1] = v1[axis1] - v0[axis1];
468 diff2[0] = v2[axis0] - v0[axis0];
469 diff2[1] = v2[axis1] - v0[axis1];
470
471 // Note: This code was rearranged somewhat from the code in
472 // Graphics Gems to provide a little more numeric
473 // stability. However, it can still miss some valid intersections
474 // on very tiny triangles.
475 isInter = FALSE;
476 beta = ((diff0[1] * diff1[0] - diff0[0] * diff1[1]) /
477 (diff2[1] * diff1[0] - diff2[0] * diff1[1]));
478 if (beta >= 0.0 && beta <= 1.0) {
479 alpha = -1.0;
480 if (diff1[1] < -EPSILON || diff1[1] > EPSILON)
481 alpha = (diff0[1] - beta * diff2[1]) / diff1[1];
482 else
483 alpha = (diff0[0] - beta * diff2[0]) / diff1[0];
484 isInter = (alpha >= 0.0 && alpha + beta <= 1.0);
485 }
486
487 //
488 // (5) If there is an intersection, set up the barycentric
489 // coordinates and figure out if the front was hit.
490 //
491 if (isInter) {
492 barycentric.setValue(1.0 - (alpha + beta), alpha, beta);
493 front = (getDirection().dot(norm) < 0.0);
494 intersection = getPosition() + t * getDirection();
495 }
496
497 return isInter;
498
499 #undef EPSILON
500 }