1 /* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
2 *
3 * This library is open source and may be redistributed and/or modified under
4 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
5 * (at your option) any later version. The full license is in LICENSE file
6 * included with this distribution, and on the openscenegraph.org website.
7 *
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * OpenSceneGraph Public License for more details.
12 */
13 #include <osg/LineSegment>
14 #include <osg/Notify>
15 #include <osg/io_utils>
16
17 using namespace osg;
18
~LineSegment()19 LineSegment::~LineSegment()
20 {
21 }
22
intersectAndClip(vec_type & s,vec_type & e,const BoundingBox & bb)23 bool LineSegment::intersectAndClip(vec_type& s,vec_type& e,const BoundingBox& bb)
24 {
25 // compate s and e against the xMin to xMax range of bb.
26 if (s.x()<=e.x())
27 {
28
29 // trivial reject of segment wholely outside.
30 if (e.x()<bb.xMin()) return false;
31 if (s.x()>bb.xMax()) return false;
32
33 if (s.x()<bb.xMin())
34 {
35 // clip s to xMin.
36 s = s+(e-s)*(bb.xMin()-s.x())/(e.x()-s.x());
37 }
38
39 if (e.x()>bb.xMax())
40 {
41 // clip e to xMax.
42 e = s+(e-s)*(bb.xMax()-s.x())/(e.x()-s.x());
43 }
44 }
45 else
46 {
47 if (s.x()<bb.xMin()) return false;
48 if (e.x()>bb.xMax()) return false;
49
50 if (e.x()<bb.xMin())
51 {
52 // clip s to xMin.
53 e = s+(e-s)*(bb.xMin()-s.x())/(e.x()-s.x());
54 }
55
56 if (s.x()>bb.xMax())
57 {
58 // clip e to xMax.
59 s = s+(e-s)*(bb.xMax()-s.x())/(e.x()-s.x());
60 }
61 }
62
63 // compate s and e against the yMin to yMax range of bb.
64 if (s.y()<=e.y())
65 {
66
67 // trivial reject of segment wholely outside.
68 if (e.y()<bb.yMin()) return false;
69 if (s.y()>bb.yMax()) return false;
70
71 if (s.y()<bb.yMin())
72 {
73 // clip s to yMin.
74 s = s+(e-s)*(bb.yMin()-s.y())/(e.y()-s.y());
75 }
76
77 if (e.y()>bb.yMax())
78 {
79 // clip e to yMax.
80 e = s+(e-s)*(bb.yMax()-s.y())/(e.y()-s.y());
81 }
82 }
83 else
84 {
85 if (s.y()<bb.yMin()) return false;
86 if (e.y()>bb.yMax()) return false;
87
88 if (e.y()<bb.yMin())
89 {
90 // clip s to yMin.
91 e = s+(e-s)*(bb.yMin()-s.y())/(e.y()-s.y());
92 }
93
94 if (s.y()>bb.yMax())
95 {
96 // clip e to yMax.
97 s = s+(e-s)*(bb.yMax()-s.y())/(e.y()-s.y());
98 }
99 }
100
101 // compate s and e against the zMin to zMax range of bb.
102 if (s.z()<=e.z())
103 {
104
105 // trivial reject of segment wholely outside.
106 if (e.z()<bb.zMin()) return false;
107 if (s.z()>bb.zMax()) return false;
108
109 if (s.z()<bb.zMin())
110 {
111 // clip s to zMin.
112 s = s+(e-s)*(bb.zMin()-s.z())/(e.z()-s.z());
113 }
114
115 if (e.z()>bb.zMax())
116 {
117 // clip e to zMax.
118 e = s+(e-s)*(bb.zMax()-s.z())/(e.z()-s.z());
119 }
120 }
121 else
122 {
123 if (s.z()<bb.zMin()) return false;
124 if (e.z()>bb.zMax()) return false;
125
126 if (e.z()<bb.zMin())
127 {
128 // clip s to zMin.
129 e = s+(e-s)*(bb.zMin()-s.z())/(e.z()-s.z());
130 }
131
132 if (s.z()>bb.zMax())
133 {
134 // clip e to zMax.
135 s = s+(e-s)*(bb.zMax()-s.z())/(e.z()-s.z());
136 }
137 }
138
139 return true;
140 }
141
intersect(const BoundingBox & bb) const142 bool LineSegment::intersect(const BoundingBox& bb) const
143 {
144 if (!bb.valid()) return false;
145
146 vec_type s=_s,e=_e;
147 return intersectAndClip(s,e,bb);
148 }
149
150
intersectAndComputeRatios(const BoundingBox & bb,float & r1,float & r2) const151 bool LineSegment::intersectAndComputeRatios(const BoundingBox& bb,float& r1,float& r2) const
152 {
153 if (!bb.valid()) return false;
154
155 vec_type s=_s,e=_e;
156 bool result = intersectAndClip(s,e,bb);
157 if (result)
158 {
159 value_type len = (_e-_s).length();
160 if (len>0.0f)
161 {
162 value_type inv_len = 1.0f/len;
163 r1 = (float)((s-_s).length()*inv_len);
164 r2 = (float)((e-_s).length()*inv_len);
165 }
166 else
167 {
168 r1 = 0.0f;
169 r2 = 0.0f;
170 }
171 }
172 return result;
173 }
174
intersectAndComputeRatios(const BoundingBox & bb,double & r1,double & r2) const175 bool LineSegment::intersectAndComputeRatios(const BoundingBox& bb,double& r1,double& r2) const
176 {
177 if (!bb.valid()) return false;
178
179 vec_type s=_s,e=_e;
180 bool result = intersectAndClip(s,e,bb);
181 if (result)
182 {
183 double len = (_e-_s).length();
184 if (len>0.0)
185 {
186 double inv_len = 1.0/len;
187 r1 = ((s-_s).length()*inv_len);
188 r2 = ((e-_s).length()*inv_len);
189
190 OSG_NOTICE<<"s = ("<<s<<"), e = ("<<e<<")"<<std::endl;
191
192 }
193 else
194 {
195 r1 = 0.0;
196 r2 = 0.0;
197 }
198 }
199 return result;
200 }
201
202
intersectAndComputeRatios(const BoundingSphere & bs,float & r1,float & r2) const203 bool LineSegment::intersectAndComputeRatios(const BoundingSphere& bs,float& r1,float& r2) const
204 {
205 vec_type sm = _s-bs._center;
206 value_type c = sm.length2()-bs._radius*bs._radius;
207
208 vec_type se = _e-_s;
209 value_type a = se.length2();
210
211
212 // check for zero length segment.
213 if (a==0.0)
214 {
215 // check if start point outside sphere radius
216 if (c>0.0) return false;
217
218 // length segment within radius of bounding sphere but zero length
219 // so return true, and set the ratio so the start point is the one
220 // to be used.
221 r1 = 1.0f;
222 r2 = 0.0f;
223 return true;
224 }
225
226 value_type b = sm*se*2.0f;
227
228 value_type d = b*b-4.0f*a*c;
229
230 if (d<0.0f) return false;
231
232 d = (value_type)sqrt(d);
233
234
235 value_type div = 1.0f/(2.0f*a);
236
237 r1 = (float)((-b-d)*div);
238 r2 = (float)((-b+d)*div);
239
240 if (r1<=0.0f && r2<=0.0f) return false;
241
242 if (r1>=1.0f && r2>=1.0f) return false;
243
244 return true;
245 }
246
247
248
intersectAndComputeRatios(const BoundingSphere & bs,double & r1,double & r2) const249 bool LineSegment::intersectAndComputeRatios(const BoundingSphere& bs,double& r1,double& r2) const
250 {
251 vec_type sm = _s-bs._center;
252 value_type c = sm.length2()-bs._radius*bs._radius;
253
254 vec_type se = _e-_s;
255 value_type a = se.length2();
256
257
258 // check for zero length segment.
259 if (a==0.0)
260 {
261 // check if start point outside sphere radius
262 if (c>0.0) return false;
263
264 // length segment within radius of bounding sphere but zero length
265 // so return true, and set the ratio so the start point is the one
266 // to be used.
267 r1 = 1.0f;
268 r2 = 0.0f;
269 return true;
270 }
271
272 value_type b = sm*se*2.0;
273
274 value_type d = b*b-4.0f*a*c;
275
276 if (d<0.0f) return false;
277
278 d = (value_type)sqrt(d);
279
280
281 value_type div = 1.0f/(2.0*a);
282
283 r1 = ((-b-d)*div);
284 r2 = ((-b+d)*div);
285
286 if (r1<=0.0 && r2<=0.0) return false;
287
288 if (r1>=1.0 && r2>=1.0) return false;
289
290 return true;
291 }
292
intersect(const BoundingSphere & bs) const293 bool LineSegment::intersect(const BoundingSphere& bs) const
294 {
295 vec_type sm = _s-bs._center;
296 value_type c = sm.length2()-bs._radius*bs._radius;
297 if (c<0.0f) return true;
298
299 vec_type se = _e-_s;
300 value_type a = se.length2();
301
302 value_type b = (sm*se)*2.0f;
303
304 value_type d = b*b-4.0f*a*c;
305
306 if (d<0.0f) return false;
307
308 d = (value_type) sqrt(d);
309
310 value_type div = 1.0f/(2.0f*a);
311
312 value_type r1 = (-b-d)*div;
313 value_type r2 = (-b+d)*div;
314
315 if (r1<=0.0f && r2<=0.0f) return false;
316
317 if (r1>=1.0f && r2>=1.0f) return false;
318
319 return true;
320 }
321
intersect(const Vec3f & v1,const Vec3f & v2,const Vec3f & v3,float & r)322 bool LineSegment::intersect(const Vec3f& v1,const Vec3f& v2,const Vec3f& v3,float& r)
323 {
324 if (v1==v2 || v2==v3 || v1==v3) return false;
325
326 vec_type vse = _e-_s;
327
328 vec_type v12 = v2-v1;
329 vec_type n12 = v12^vse;
330 value_type ds12 = (_s-v1)*n12;
331 value_type d312 = (v3-v1)*n12;
332 if (d312>=0.0)
333 {
334 if (ds12<0.0) return false;
335 if (ds12>d312) return false;
336 }
337 else // d312 < 0
338 {
339 if (ds12>0.0) return false;
340 if (ds12<d312) return false;
341 }
342
343 vec_type v23 = v3-v2;
344 vec_type n23 = v23^vse;
345 value_type ds23 = (_s-v2)*n23;
346 value_type d123 = (v1-v2)*n23;
347 if (d123>=0.0)
348 {
349 if (ds23<0.0) return false;
350 if (ds23>d123) return false;
351 }
352 else // d123 < 0
353 {
354 if (ds23>0.0) return false;
355 if (ds23<d123) return false;
356 }
357
358 vec_type v31 = v1-v3;
359 vec_type n31 = v31^vse;
360 value_type ds31 = (_s-v3)*n31;
361 value_type d231 = (v2-v3)*n31;
362 if (d231>=0.0)
363 {
364 if (ds31<0.0) return false;
365 if (ds31>d231) return false;
366 }
367 else // d231 < 0
368 {
369 if (ds31>0.0) return false;
370 if (ds31<d231) return false;
371 }
372
373 value_type r3 = ds12/d312;
374 value_type r1 = ds23/d123;
375 value_type r2 = ds31/d231;
376
377 // value_type rt = r1+r2+r3;
378
379 vec_type in = v1*r1+v2*r2+v3*r3;
380
381 value_type length = vse.length();
382 vse /= length;
383 value_type d = (in-_s)*vse;
384
385 if (d<0.0) return false;
386 if (d>length) return false;
387
388 r = (float) d/length;
389
390 return true;
391 }
392
intersect(const Vec3d & v1,const Vec3d & v2,const Vec3d & v3,double & r)393 bool LineSegment::intersect(const Vec3d& v1,const Vec3d& v2,const Vec3d& v3,double& r)
394 {
395 if (v1==v2 || v2==v3 || v1==v3) return false;
396
397 vec_type vse = _e-_s;
398
399 vec_type v12 = v2-v1;
400 vec_type n12 = v12^vse;
401 value_type ds12 = (_s-v1)*n12;
402 value_type d312 = (v3-v1)*n12;
403 if (d312>=0.0)
404 {
405 if (ds12<0.0) return false;
406 if (ds12>d312) return false;
407 }
408 else // d312 < 0
409 {
410 if (ds12>0.0) return false;
411 if (ds12<d312) return false;
412 }
413
414 vec_type v23 = v3-v2;
415 vec_type n23 = v23^vse;
416 value_type ds23 = (_s-v2)*n23;
417 value_type d123 = (v1-v2)*n23;
418 if (d123>=0.0)
419 {
420 if (ds23<0.0) return false;
421 if (ds23>d123) return false;
422 }
423 else // d123 < 0
424 {
425 if (ds23>0.0) return false;
426 if (ds23<d123) return false;
427 }
428
429 vec_type v31 = v1-v3;
430 vec_type n31 = v31^vse;
431 value_type ds31 = (_s-v3)*n31;
432 value_type d231 = (v2-v3)*n31;
433 if (d231>=0.0)
434 {
435 if (ds31<0.0) return false;
436 if (ds31>d231) return false;
437 }
438 else // d231 < 0
439 {
440 if (ds31>0.0) return false;
441 if (ds31<d231) return false;
442 }
443
444 value_type r3 = ds12/d312;
445 value_type r1 = ds23/d123;
446 value_type r2 = ds31/d231;
447
448 // value_type rt = r1+r2+r3;
449
450 vec_type in = v1*r1+v2*r2+v3*r3;
451
452 value_type length = vse.length();
453 vse /= length;
454 value_type d = (in-_s)*vse;
455
456 if (d<0.0) return false;
457 if (d>length) return false;
458
459 r = d/length;
460
461 return true;
462 }
463