1 /*
2 -----------------------------------------------------------------------------
3 This source file is part of OGRE
4 (Object-oriented Graphics Rendering Engine)
5 For the latest info, see http://www.ogre3d.org/
6
7 Copyright (c) 2000-2013 Torus Knot Software Ltd
8
9 Permission is hereby granted, free of charge, to any person obtaining a copy
10 of this software and associated documentation files (the "Software"), to deal
11 in the Software without restriction, including without limitation the rights
12 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13 copies of the Software, and to permit persons to whom the Software is
14 furnished to do so, subject to the following conditions:
15
16 The above copyright notice and this permission notice shall be included in
17 all copies or substantial portions of the Software.
18
19 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
25 THE SOFTWARE.
26 -----------------------------------------------------------------------------
27 OgreSegment.cpp - 3D Line Segment class for intersection testing in Ogre3D
28 Some algorithms based off code from the Wild Magic library by Dave Eberly
29 -----------------------------------------------------------------------------
30 begin : Mon Apr 02 2007
31 author : Eric Cha
32 email : ericc@xenopi.com
33 Code Style Update :
34 -----------------------------------------------------------------------------
35 */
36
37 #include "OgreSegment.h"
38 #include "OgreCapsule.h"
39
40 using namespace Ogre;
41
42 const Real PARALLEL_TOLERANCE = 0.0001;
43
44 //----------------------------------------------------------------------------
Segment()45 Segment::Segment ()
46 {
47 // uninitialized
48 }
49 //----------------------------------------------------------------------------
Segment(const Vector3 & origin,const Vector3 & direction,Real extent)50 Segment::Segment (const Vector3& origin,
51 const Vector3& direction,
52 Real extent)
53 :
54 mOrigin(origin),
55 mDirection(direction),
56 mExtent(extent)
57 {
58 }
59 //----------------------------------------------------------------------------
set(const Vector3 & newOrigin,const Vector3 & newEnd)60 void Segment::set(const Vector3& newOrigin, const Vector3& newEnd)
61 {
62 mOrigin = newOrigin;
63 // calc the direction vector
64 mDirection = newEnd - mOrigin;
65 mExtent = mDirection.normalise();
66 }
67 //----------------------------------------------------------------------------
setOrigin(const Vector3 & newOrigin)68 void Segment::setOrigin(const Vector3& newOrigin)
69 {
70 mOrigin = newOrigin;
71 }
72 //----------------------------------------------------------------------------
setEndPoint(const Vector3 & newEnd)73 void Segment::setEndPoint(const Vector3& newEnd)
74 {
75 // calc the direction vector
76 mDirection = newEnd - mOrigin;
77 mExtent = mDirection.normalise();
78 }
79 //----------------------------------------------------------------------------
distance(const Segment & otherSegment) const80 Real Segment::distance(const Segment& otherSegment) const
81 {
82 Real fSqrDist = squaredDistance(otherSegment);
83 return Ogre::Math::Sqrt(fSqrDist);
84 }
85 //----------------------------------------------------------------------------
squaredDistance(const Segment & otherSegment) const86 Real Segment::squaredDistance(const Segment& otherSegment) const
87 {
88 Vector3 kDiff = mOrigin - otherSegment.mOrigin;
89 Real fA01 = -mDirection.dotProduct(otherSegment.mDirection);
90 Real fB0 = kDiff.dotProduct(mDirection);
91 Real fB1 = -kDiff.dotProduct(otherSegment.mDirection);
92 Real fC = kDiff.squaredLength();
93 Real fDet = Math::Abs((Real)1.0 - fA01*fA01);
94 Real fS0, fS1, fSqrDist, fExtDet0, fExtDet1, fTmpS0, fTmpS1;
95
96 if (fDet >= PARALLEL_TOLERANCE)
97 {
98 // segments are not parallel
99 fS0 = fA01*fB1-fB0;
100 fS1 = fA01*fB0-fB1;
101 fExtDet0 = mExtent*fDet;
102 fExtDet1 = otherSegment.mExtent*fDet;
103
104 if (fS0 >= -fExtDet0)
105 {
106 if (fS0 <= fExtDet0)
107 {
108 if (fS1 >= -fExtDet1)
109 {
110 if (fS1 <= fExtDet1) // region 0 (interior)
111 {
112 // minimum at two interior points of 3D lines
113 Real fInvDet = ((Real)1.0)/fDet;
114 fS0 *= fInvDet;
115 fS1 *= fInvDet;
116 fSqrDist = fS0*(fS0+fA01*fS1+((Real)2.0)*fB0) +
117 fS1*(fA01*fS0+fS1+((Real)2.0)*fB1)+fC;
118 }
119 else // region 3 (side)
120 {
121 fS1 = otherSegment.mExtent;
122 fTmpS0 = -(fA01*fS1+fB0);
123 if (fTmpS0 < -mExtent)
124 {
125 fS0 = -mExtent;
126 fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
127 fS1*(fS1+((Real)2.0)*fB1)+fC;
128 }
129 else if (fTmpS0 <= mExtent)
130 {
131 fS0 = fTmpS0;
132 fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
133 }
134 else
135 {
136 fS0 = mExtent;
137 fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
138 fS1*(fS1+((Real)2.0)*fB1)+fC;
139 }
140 }
141 }
142 else // region 7 (side)
143 {
144 fS1 = -otherSegment.mExtent;
145 fTmpS0 = -(fA01*fS1+fB0);
146 if (fTmpS0 < -mExtent)
147 {
148 fS0 = -mExtent;
149 fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
150 fS1*(fS1+((Real)2.0)*fB1)+fC;
151 }
152 else if (fTmpS0 <= mExtent)
153 {
154 fS0 = fTmpS0;
155 fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
156 }
157 else
158 {
159 fS0 = mExtent;
160 fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
161 fS1*(fS1+((Real)2.0)*fB1)+fC;
162 }
163 }
164 }
165 else
166 {
167 if (fS1 >= -fExtDet1)
168 {
169 if (fS1 <= fExtDet1) // region 1 (side)
170 {
171 fS0 = mExtent;
172 fTmpS1 = -(fA01*fS0+fB1);
173 if (fTmpS1 < -otherSegment.mExtent)
174 {
175 fS1 = -otherSegment.mExtent;
176 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
177 fS0*(fS0+((Real)2.0)*fB0)+fC;
178 }
179 else if (fTmpS1 <= otherSegment.mExtent)
180 {
181 fS1 = fTmpS1;
182 fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)+fC;
183 }
184 else
185 {
186 fS1 = otherSegment.mExtent;
187 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
188 fS0*(fS0+((Real)2.0)*fB0)+fC;
189 }
190 }
191 else // region 2 (corner)
192 {
193 fS1 = otherSegment.mExtent;
194 fTmpS0 = -(fA01*fS1+fB0);
195 if (fTmpS0 < -mExtent)
196 {
197 fS0 = -mExtent;
198 fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
199 fS1*(fS1+((Real)2.0)*fB1)+fC;
200 }
201 else if (fTmpS0 <= mExtent)
202 {
203 fS0 = fTmpS0;
204 fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
205 }
206 else
207 {
208 fS0 = mExtent;
209 fTmpS1 = -(fA01*fS0+fB1);
210 if (fTmpS1 < -otherSegment.mExtent)
211 {
212 fS1 = -otherSegment.mExtent;
213 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
214 fS0*(fS0+((Real)2.0)*fB0)+fC;
215 }
216 else if (fTmpS1 <= otherSegment.mExtent)
217 {
218 fS1 = fTmpS1;
219 fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)
220 + fC;
221 }
222 else
223 {
224 fS1 = otherSegment.mExtent;
225 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
226 fS0*(fS0+((Real)2.0)*fB0)+fC;
227 }
228 }
229 }
230 }
231 else // region 8 (corner)
232 {
233 fS1 = -otherSegment.mExtent;
234 fTmpS0 = -(fA01*fS1+fB0);
235 if (fTmpS0 < -mExtent)
236 {
237 fS0 = -mExtent;
238 fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
239 fS1*(fS1+((Real)2.0)*fB1)+fC;
240 }
241 else if (fTmpS0 <= mExtent)
242 {
243 fS0 = fTmpS0;
244 fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
245 }
246 else
247 {
248 fS0 = mExtent;
249 fTmpS1 = -(fA01*fS0+fB1);
250 if (fTmpS1 > otherSegment.mExtent)
251 {
252 fS1 = otherSegment.mExtent;
253 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
254 fS0*(fS0+((Real)2.0)*fB0)+fC;
255 }
256 else if (fTmpS1 >= -otherSegment.mExtent)
257 {
258 fS1 = fTmpS1;
259 fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)
260 + fC;
261 }
262 else
263 {
264 fS1 = -otherSegment.mExtent;
265 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
266 fS0*(fS0+((Real)2.0)*fB0)+fC;
267 }
268 }
269 }
270 }
271 }
272 else
273 {
274 if (fS1 >= -fExtDet1)
275 {
276 if (fS1 <= fExtDet1) // region 5 (side)
277 {
278 fS0 = -mExtent;
279 fTmpS1 = -(fA01*fS0+fB1);
280 if (fTmpS1 < -otherSegment.mExtent)
281 {
282 fS1 = -otherSegment.mExtent;
283 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
284 fS0*(fS0+((Real)2.0)*fB0)+fC;
285 }
286 else if (fTmpS1 <= otherSegment.mExtent)
287 {
288 fS1 = fTmpS1;
289 fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)+fC;
290 }
291 else
292 {
293 fS1 = otherSegment.mExtent;
294 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
295 fS0*(fS0+((Real)2.0)*fB0)+fC;
296 }
297 }
298 else // region 4 (corner)
299 {
300 fS1 = otherSegment.mExtent;
301 fTmpS0 = -(fA01*fS1+fB0);
302 if (fTmpS0 > mExtent)
303 {
304 fS0 = mExtent;
305 fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
306 fS1*(fS1+((Real)2.0)*fB1)+fC;
307 }
308 else if (fTmpS0 >= -mExtent)
309 {
310 fS0 = fTmpS0;
311 fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
312 }
313 else
314 {
315 fS0 = -mExtent;
316 fTmpS1 = -(fA01*fS0+fB1);
317 if (fTmpS1 < -otherSegment.mExtent)
318 {
319 fS1 = -otherSegment.mExtent;
320 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
321 fS0*(fS0+((Real)2.0)*fB0)+fC;
322 }
323 else if (fTmpS1 <= otherSegment.mExtent)
324 {
325 fS1 = fTmpS1;
326 fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)
327 + fC;
328 }
329 else
330 {
331 fS1 = otherSegment.mExtent;
332 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
333 fS0*(fS0+((Real)2.0)*fB0)+fC;
334 }
335 }
336 }
337 }
338 else // region 6 (corner)
339 {
340 fS1 = -otherSegment.mExtent;
341 fTmpS0 = -(fA01*fS1+fB0);
342 if (fTmpS0 > mExtent)
343 {
344 fS0 = mExtent;
345 fSqrDist = fS0*(fS0-((Real)2.0)*fTmpS0) +
346 fS1*(fS1+((Real)2.0)*fB1)+fC;
347 }
348 else if (fTmpS0 >= -mExtent)
349 {
350 fS0 = fTmpS0;
351 fSqrDist = -fS0*fS0+fS1*(fS1+((Real)2.0)*fB1)+fC;
352 }
353 else
354 {
355 fS0 = -mExtent;
356 fTmpS1 = -(fA01*fS0+fB1);
357 if (fTmpS1 < -otherSegment.mExtent)
358 {
359 fS1 = -otherSegment.mExtent;
360 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
361 fS0*(fS0+((Real)2.0)*fB0)+fC;
362 }
363 else if (fTmpS1 <= otherSegment.mExtent)
364 {
365 fS1 = fTmpS1;
366 fSqrDist = -fS1*fS1+fS0*(fS0+((Real)2.0)*fB0)
367 + fC;
368 }
369 else
370 {
371 fS1 = otherSegment.mExtent;
372 fSqrDist = fS1*(fS1-((Real)2.0)*fTmpS1) +
373 fS0*(fS0+((Real)2.0)*fB0)+fC;
374 }
375 }
376 }
377 }
378 }
379 else
380 {
381 // The segments are parallel. The average b0 term is designed to
382 // ensure symmetry of the function. That is, dist(seg0,seg1) and
383 // dist(seg1,seg0) should produce the same number.
384 Real fE0pE1 = mExtent + otherSegment.mExtent;
385 Real fSign = (fA01 > (Real)0.0 ? (Real)-1.0 : (Real)1.0);
386 Real fB0Avr = ((Real)0.5)*(fB0 - fSign*fB1);
387 Real fLambda = -fB0Avr;
388 if (fLambda < -fE0pE1)
389 {
390 fLambda = -fE0pE1;
391 }
392 else if (fLambda > fE0pE1)
393 {
394 fLambda = fE0pE1;
395 }
396
397 // fS1 = -fSign*fLambda*otherSegment.mExtent/fE0pE1;
398 // fS0 = fLambda + fSign*fS1;
399 fSqrDist = fLambda*(fLambda + ((Real)2.0)*fB0Avr) + fC;
400 }
401 // we don't need the following stuff - it's for calculating closest point
402 // mClosestPoint0 = mOrigin + fS0*mDirection;
403 // mClosestPoint1 = otherSegment.mOrigin + fS1*otherSegment.mDirection;
404 // mSegment0Parameter = fS0;
405 // mSegment1Parameter = fS1;
406 return Math::Abs(fSqrDist);
407 }
408
409 //----------------------------------------------------------------------------
intersects(const Capsule & capsule) const410 bool Segment::intersects(const Capsule &capsule) const
411 {
412 Real fDist = distance(capsule.mSegment);
413 return fDist <= capsule.mRadius;
414 }
415