1 // Following code from Magic-Software (http://www.magic-software.com/)
2 // A bit modified for Opcode
3
4 static const float gs_fTolerance = 1e-05f;
5
OPC_PointTriangleSqrDist(const Point & point,const Point & p0,const Point & p1,const Point & p2)6 static float OPC_PointTriangleSqrDist(const Point& point, const Point& p0, const Point& p1, const Point& p2)
7 {
8 // Hook
9 Point TriEdge0 = p1 - p0;
10 Point TriEdge1 = p2 - p0;
11
12 Point kDiff = p0 - point;
13 float fA00 = TriEdge0.SquareMagnitude();
14 float fA01 = TriEdge0 | TriEdge1;
15 float fA11 = TriEdge1.SquareMagnitude();
16 float fB0 = kDiff | TriEdge0;
17 float fB1 = kDiff | TriEdge1;
18 float fC = kDiff.SquareMagnitude();
19 float fDet = fabsf(fA00*fA11 - fA01*fA01);
20 float fS = fA01*fB1-fA11*fB0;
21 float fT = fA01*fB0-fA00*fB1;
22 float fSqrDist;
23
24 if(fS + fT <= fDet)
25 {
26 if(fS < 0.0f)
27 {
28 if(fT < 0.0f) // region 4
29 {
30 if(fB0 < 0.0f)
31 {
32 if(-fB0 >= fA00) fSqrDist = fA00+2.0f*fB0+fC;
33 else fSqrDist = fB0*(-fB0/fA00)+fC;
34 }
35 else
36 {
37 if(fB1 >= 0.0f) fSqrDist = fC;
38 else if(-fB1 >= fA11) fSqrDist = fA11+2.0f*fB1+fC;
39 else fSqrDist = fB1*(-fB1/fA11)+fC;
40 }
41 }
42 else // region 3
43 {
44 if(fB1 >= 0.0f) fSqrDist = fC;
45 else if(-fB1 >= fA11) fSqrDist = fA11+2.0f*fB1+fC;
46 else fSqrDist = fB1*(-fB1/fA11)+fC;
47 }
48 }
49 else if(fT < 0.0f) // region 5
50 {
51 if(fB0 >= 0.0f) fSqrDist = fC;
52 else if(-fB0 >= fA00) fSqrDist = fA00+2.0f*fB0+fC;
53 else fSqrDist = fB0*(-fB0/fA00)+fC;
54 }
55 else // region 0
56 {
57 // minimum at interior point
58 if(fDet==0.0f)
59 {
60 fSqrDist = MAX_FLOAT;
61 }
62 else
63 {
64 float fInvDet = 1.0f/fDet;
65 fS *= fInvDet;
66 fT *= fInvDet;
67 fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
68 }
69 }
70 }
71 else
72 {
73 float fTmp0, fTmp1, fNumer, fDenom;
74
75 if(fS < 0.0f) // region 2
76 {
77 fTmp0 = fA01 + fB0;
78 fTmp1 = fA11 + fB1;
79 if(fTmp1 > fTmp0)
80 {
81 fNumer = fTmp1 - fTmp0;
82 fDenom = fA00-2.0f*fA01+fA11;
83 if(fNumer >= fDenom)
84 {
85 fSqrDist = fA00+2.0f*fB0+fC;
86 }
87 else
88 {
89 fS = fNumer/fDenom;
90 fT = 1.0f - fS;
91 fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
92 }
93 }
94 else
95 {
96 if(fTmp1 <= 0.0f) fSqrDist = fA11+2.0f*fB1+fC;
97 else if(fB1 >= 0.0f) fSqrDist = fC;
98 else fSqrDist = fB1*(-fB1/fA11)+fC;
99 }
100 }
101 else if(fT < 0.0f) // region 6
102 {
103 fTmp0 = fA01 + fB1;
104 fTmp1 = fA00 + fB0;
105 if(fTmp1 > fTmp0)
106 {
107 fNumer = fTmp1 - fTmp0;
108 fDenom = fA00-2.0f*fA01+fA11;
109 if(fNumer >= fDenom)
110 {
111 fSqrDist = fA11+2.0f*fB1+fC;
112 }
113 else
114 {
115 fT = fNumer/fDenom;
116 fS = 1.0f - fT;
117 fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
118 }
119 }
120 else
121 {
122 if(fTmp1 <= 0.0f) fSqrDist = fA00+2.0f*fB0+fC;
123 else if(fB0 >= 0.0f) fSqrDist = fC;
124 else fSqrDist = fB0*(-fB0/fA00)+fC;
125 }
126 }
127 else // region 1
128 {
129 fNumer = fA11 + fB1 - fA01 - fB0;
130 if(fNumer <= 0.0f)
131 {
132 fSqrDist = fA11+2.0f*fB1+fC;
133 }
134 else
135 {
136 fDenom = fA00-2.0f*fA01+fA11;
137 if(fNumer >= fDenom)
138 {
139 fSqrDist = fA00+2.0f*fB0+fC;
140 }
141 else
142 {
143 fS = fNumer/fDenom;
144 fT = 1.0f - fS;
145 fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
146 }
147 }
148 }
149 }
150 return fabsf(fSqrDist);
151 }
152
OPC_SegmentSegmentSqrDist(const Segment & rkSeg0,const Segment & rkSeg1)153 static float OPC_SegmentSegmentSqrDist(const Segment& rkSeg0, const Segment& rkSeg1)
154 {
155 // Hook
156 Point rkSeg0Direction = rkSeg0.ComputeDirection();
157 Point rkSeg1Direction = rkSeg1.ComputeDirection();
158
159 Point kDiff = rkSeg0.mP0 - rkSeg1.mP0;
160 float fA00 = rkSeg0Direction.SquareMagnitude();
161 float fA01 = -rkSeg0Direction.Dot(rkSeg1Direction);
162 float fA11 = rkSeg1Direction.SquareMagnitude();
163 float fB0 = kDiff.Dot(rkSeg0Direction);
164 float fC = kDiff.SquareMagnitude();
165 float fDet = fabsf(fA00*fA11-fA01*fA01);
166
167 float fB1, fS, fT, fSqrDist, fTmp;
168
169 if(fDet>=gs_fTolerance)
170 {
171 // line segments are not parallel
172 fB1 = -kDiff.Dot(rkSeg1Direction);
173 fS = fA01*fB1-fA11*fB0;
174 fT = fA01*fB0-fA00*fB1;
175
176 if(fS >= 0.0f)
177 {
178 if(fS <= fDet)
179 {
180 if(fT >= 0.0f)
181 {
182 if(fT <= fDet) // region 0 (interior)
183 {
184 // minimum at two interior points of 3D lines
185 float fInvDet = 1.0f/fDet;
186 fS *= fInvDet;
187 fT *= fInvDet;
188 fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) + fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
189 }
190 else // region 3 (side)
191 {
192 fTmp = fA01+fB0;
193 if(fTmp>=0.0f) fSqrDist = fA11+2.0f*fB1+fC;
194 else if(-fTmp>=fA00) fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
195 else fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
196 }
197 }
198 else // region 7 (side)
199 {
200 if(fB0>=0.0f) fSqrDist = fC;
201 else if(-fB0>=fA00) fSqrDist = fA00+2.0f*fB0+fC;
202 else fSqrDist = fB0*(-fB0/fA00)+fC;
203 }
204 }
205 else
206 {
207 if ( fT >= 0.0 )
208 {
209 if ( fT <= fDet ) // region 1 (side)
210 {
211 fTmp = fA01+fB1;
212 if(fTmp>=0.0f) fSqrDist = fA00+2.0f*fB0+fC;
213 else if(-fTmp>=fA11) fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
214 else fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
215 }
216 else // region 2 (corner)
217 {
218 fTmp = fA01+fB0;
219 if ( -fTmp <= fA00 )
220 {
221 if(fTmp>=0.0f) fSqrDist = fA11+2.0f*fB1+fC;
222 else fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
223 }
224 else
225 {
226 fTmp = fA01+fB1;
227 if(fTmp>=0.0f) fSqrDist = fA00+2.0f*fB0+fC;
228 else if(-fTmp>=fA11) fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
229 else fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
230 }
231 }
232 }
233 else // region 8 (corner)
234 {
235 if ( -fB0 < fA00 )
236 {
237 if(fB0>=0.0f) fSqrDist = fC;
238 else fSqrDist = fB0*(-fB0/fA00)+fC;
239 }
240 else
241 {
242 fTmp = fA01+fB1;
243 if(fTmp>=0.0f) fSqrDist = fA00+2.0f*fB0+fC;
244 else if(-fTmp>=fA11) fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
245 else fSqrDist = fTmp*(-fTmp/fA11)+fA00+2.0f*fB0+fC;
246 }
247 }
248 }
249 }
250 else
251 {
252 if ( fT >= 0.0f )
253 {
254 if ( fT <= fDet ) // region 5 (side)
255 {
256 if(fB1>=0.0f) fSqrDist = fC;
257 else if(-fB1>=fA11) fSqrDist = fA11+2.0f*fB1+fC;
258 else fSqrDist = fB1*(-fB1/fA11)+fC;
259 }
260 else // region 4 (corner)
261 {
262 fTmp = fA01+fB0;
263 if ( fTmp < 0.0f )
264 {
265 if(-fTmp>=fA00) fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
266 else fSqrDist = fTmp*(-fTmp/fA00)+fA11+2.0f*fB1+fC;
267 }
268 else
269 {
270 if(fB1>=0.0f) fSqrDist = fC;
271 else if(-fB1>=fA11) fSqrDist = fA11+2.0f*fB1+fC;
272 else fSqrDist = fB1*(-fB1/fA11)+fC;
273 }
274 }
275 }
276 else // region 6 (corner)
277 {
278 if ( fB0 < 0.0f )
279 {
280 if(-fB0>=fA00) fSqrDist = fA00+2.0f*fB0+fC;
281 else fSqrDist = fB0*(-fB0/fA00)+fC;
282 }
283 else
284 {
285 if(fB1>=0.0f) fSqrDist = fC;
286 else if(-fB1>=fA11) fSqrDist = fA11+2.0f*fB1+fC;
287 else fSqrDist = fB1*(-fB1/fA11)+fC;
288 }
289 }
290 }
291 }
292 else
293 {
294 // line segments are parallel
295 if ( fA01 > 0.0f )
296 {
297 // direction vectors form an obtuse angle
298 if ( fB0 >= 0.0f )
299 {
300 fSqrDist = fC;
301 }
302 else if ( -fB0 <= fA00 )
303 {
304 fSqrDist = fB0*(-fB0/fA00)+fC;
305 }
306 else
307 {
308 fB1 = -kDiff.Dot(rkSeg1Direction);
309 fTmp = fA00+fB0;
310 if ( -fTmp >= fA01 )
311 {
312 fSqrDist = fA00+fA11+fC+2.0f*(fA01+fB0+fB1);
313 }
314 else
315 {
316 fT = -fTmp/fA01;
317 fSqrDist = fA00+2.0f*fB0+fC+fT*(fA11*fT+2.0f*(fA01+fB1));
318 }
319 }
320 }
321 else
322 {
323 // direction vectors form an acute angle
324 if ( -fB0 >= fA00 )
325 {
326 fSqrDist = fA00+2.0f*fB0+fC;
327 }
328 else if ( fB0 <= 0.0f )
329 {
330 fSqrDist = fB0*(-fB0/fA00)+fC;
331 }
332 else
333 {
334 fB1 = -kDiff.Dot(rkSeg1Direction);
335 if ( fB0 >= -fA01 )
336 {
337 fSqrDist = fA11+2.0f*fB1+fC;
338 }
339 else
340 {
341 fT = -fB0/fA01;
342 fSqrDist = fC+fT*(2.0f*fB1+fA11*fT);
343 }
344 }
345 }
346 }
347 return fabsf(fSqrDist);
348 }
349
OPC_SegmentRaySqrDist(const Segment & rkSeg0,const Ray & rkSeg1)350 inline_ float OPC_SegmentRaySqrDist(const Segment& rkSeg0, const Ray& rkSeg1)
351 {
352 return OPC_SegmentSegmentSqrDist(rkSeg0, Segment(rkSeg1.mOrig, rkSeg1.mOrig + rkSeg1.mDir));
353 }
354
OPC_SegmentTriangleSqrDist(const Segment & segment,const Point & p0,const Point & p1,const Point & p2)355 static float OPC_SegmentTriangleSqrDist(const Segment& segment, const Point& p0, const Point& p1, const Point& p2)
356 {
357 // Hook
358 const Point TriEdge0 = p1 - p0;
359 const Point TriEdge1 = p2 - p0;
360
361 const Point& rkSegOrigin = segment.GetOrigin();
362 Point rkSegDirection = segment.ComputeDirection();
363
364 Point kDiff = p0 - rkSegOrigin;
365 float fA00 = rkSegDirection.SquareMagnitude();
366 float fA01 = -rkSegDirection.Dot(TriEdge0);
367 float fA02 = -rkSegDirection.Dot(TriEdge1);
368 float fA11 = TriEdge0.SquareMagnitude();
369 float fA12 = TriEdge0.Dot(TriEdge1);
370 float fA22 = TriEdge1.Dot(TriEdge1);
371 float fB0 = -kDiff.Dot(rkSegDirection);
372 float fB1 = kDiff.Dot(TriEdge0);
373 float fB2 = kDiff.Dot(TriEdge1);
374 float fCof00 = fA11*fA22-fA12*fA12;
375 float fCof01 = fA02*fA12-fA01*fA22;
376 float fCof02 = fA01*fA12-fA02*fA11;
377 float fDet = fA00*fCof00+fA01*fCof01+fA02*fCof02;
378
379 Ray kTriSeg;
380 Point kPt;
381 float fSqrDist, fSqrDist0;
382
383 if(fabsf(fDet)>=gs_fTolerance)
384 {
385 float fCof11 = fA00*fA22-fA02*fA02;
386 float fCof12 = fA02*fA01-fA00*fA12;
387 float fCof22 = fA00*fA11-fA01*fA01;
388 float fInvDet = 1.0f/fDet;
389 float fRhs0 = -fB0*fInvDet;
390 float fRhs1 = -fB1*fInvDet;
391 float fRhs2 = -fB2*fInvDet;
392
393 float fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
394 float fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
395 float fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;
396
397 if ( fR < 0.0f )
398 {
399 if ( fS+fT <= 1.0f )
400 {
401 if ( fS < 0.0f )
402 {
403 if ( fT < 0.0f ) // region 4m
404 {
405 // min on face s=0 or t=0 or r=0
406 kTriSeg.mOrig = p0;
407 kTriSeg.mDir = TriEdge1;
408 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
409 kTriSeg.mOrig = p0;
410 kTriSeg.mDir = TriEdge0;
411 fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
412 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
413 fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
414 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
415 }
416 else // region 3m
417 {
418 // min on face s=0 or r=0
419 kTriSeg.mOrig = p0;
420 kTriSeg.mDir = TriEdge1;
421 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
422 fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
423 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
424 }
425 }
426 else if ( fT < 0.0f ) // region 5m
427 {
428 // min on face t=0 or r=0
429 kTriSeg.mOrig = p0;
430 kTriSeg.mDir = TriEdge0;
431 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
432 fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
433 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
434 }
435 else // region 0m
436 {
437 // min on face r=0
438 fSqrDist = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
439 }
440 }
441 else
442 {
443 if ( fS < 0.0f ) // region 2m
444 {
445 // min on face s=0 or s+t=1 or r=0
446 kTriSeg.mOrig = p0;
447 kTriSeg.mDir = TriEdge1;
448 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
449 kTriSeg.mOrig = p1;
450 kTriSeg.mDir = TriEdge1-TriEdge0;
451 fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
452 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
453 fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
454 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
455 }
456 else if ( fT < 0.0f ) // region 6m
457 {
458 // min on face t=0 or s+t=1 or r=0
459 kTriSeg.mOrig = p0;
460 kTriSeg.mDir = TriEdge0;
461 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
462 kTriSeg.mOrig = p1;
463 kTriSeg.mDir = TriEdge1-TriEdge0;
464 fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
465 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
466 fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
467 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
468 }
469 else // region 1m
470 {
471 // min on face s+t=1 or r=0
472 kTriSeg.mOrig = p1;
473 kTriSeg.mDir = TriEdge1-TriEdge0;
474 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
475 fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
476 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
477 }
478 }
479 }
480 else if ( fR <= 1.0f )
481 {
482 if ( fS+fT <= 1.0f )
483 {
484 if ( fS < 0.0f )
485 {
486 if ( fT < 0.0f ) // region 4
487 {
488 // min on face s=0 or t=0
489 kTriSeg.mOrig = p0;
490 kTriSeg.mDir = TriEdge1;
491 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
492 kTriSeg.mOrig = p0;
493 kTriSeg.mDir = TriEdge0;
494 fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
495 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
496 }
497 else // region 3
498 {
499 // min on face s=0
500 kTriSeg.mOrig = p0;
501 kTriSeg.mDir = TriEdge1;
502 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
503 }
504 }
505 else if ( fT < 0.0f ) // region 5
506 {
507 // min on face t=0
508 kTriSeg.mOrig = p0;
509 kTriSeg.mDir = TriEdge0;
510 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
511 }
512 else // region 0
513 {
514 // global minimum is interior, done
515 fSqrDist = fR*(fA00*fR+fA01*fS+fA02*fT+2.0f*fB0)
516 +fS*(fA01*fR+fA11*fS+fA12*fT+2.0f*fB1)
517 +fT*(fA02*fR+fA12*fS+fA22*fT+2.0f*fB2)
518 +kDiff.SquareMagnitude();
519 }
520 }
521 else
522 {
523 if ( fS < 0.0f ) // region 2
524 {
525 // min on face s=0 or s+t=1
526 kTriSeg.mOrig = p0;
527 kTriSeg.mDir = TriEdge1;
528 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
529 kTriSeg.mOrig = p1;
530 kTriSeg.mDir = TriEdge1-TriEdge0;
531 fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
532 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
533 }
534 else if ( fT < 0.0f ) // region 6
535 {
536 // min on face t=0 or s+t=1
537 kTriSeg.mOrig = p0;
538 kTriSeg.mDir = TriEdge0;
539 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
540 kTriSeg.mOrig = p1;
541 kTriSeg.mDir = TriEdge1-TriEdge0;
542 fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
543 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
544 }
545 else // region 1
546 {
547 // min on face s+t=1
548 kTriSeg.mOrig = p1;
549 kTriSeg.mDir = TriEdge1-TriEdge0;
550 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
551 }
552 }
553 }
554 else // fR > 1
555 {
556 if ( fS+fT <= 1.0f )
557 {
558 if ( fS < 0.0f )
559 {
560 if ( fT < 0.0f ) // region 4p
561 {
562 // min on face s=0 or t=0 or r=1
563 kTriSeg.mOrig = p0;
564 kTriSeg.mDir = TriEdge1;
565 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
566 kTriSeg.mOrig = p0;
567 kTriSeg.mDir = TriEdge0;
568 fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
569 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
570 kPt = rkSegOrigin+rkSegDirection;
571 fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
572 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
573 }
574 else // region 3p
575 {
576 // min on face s=0 or r=1
577 kTriSeg.mOrig = p0;
578 kTriSeg.mDir = TriEdge1;
579 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
580 kPt = rkSegOrigin+rkSegDirection;
581 fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
582 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
583 }
584 }
585 else if ( fT < 0.0f ) // region 5p
586 {
587 // min on face t=0 or r=1
588 kTriSeg.mOrig = p0;
589 kTriSeg.mDir = TriEdge0;
590 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
591 kPt = rkSegOrigin+rkSegDirection;
592 fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
593 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
594 }
595 else // region 0p
596 {
597 // min face on r=1
598 kPt = rkSegOrigin+rkSegDirection;
599 fSqrDist = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
600 }
601 }
602 else
603 {
604 if ( fS < 0.0f ) // region 2p
605 {
606 // min on face s=0 or s+t=1 or r=1
607 kTriSeg.mOrig = p0;
608 kTriSeg.mDir = TriEdge1;
609 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
610 kTriSeg.mOrig = p1;
611 kTriSeg.mDir = TriEdge1-TriEdge0;
612 fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
613 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
614 kPt = rkSegOrigin+rkSegDirection;
615 fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
616 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
617 }
618 else if ( fT < 0.0f ) // region 6p
619 {
620 // min on face t=0 or s+t=1 or r=1
621 kTriSeg.mOrig = p0;
622 kTriSeg.mDir = TriEdge0;
623 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
624 kTriSeg.mOrig = p1;
625 kTriSeg.mDir = TriEdge1-TriEdge0;
626 fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
627 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
628 kPt = rkSegOrigin+rkSegDirection;
629 fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
630 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
631 }
632 else // region 1p
633 {
634 // min on face s+t=1 or r=1
635 kTriSeg.mOrig = p1;
636 kTriSeg.mDir = TriEdge1-TriEdge0;
637 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
638 kPt = rkSegOrigin+rkSegDirection;
639 fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
640 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
641 }
642 }
643 }
644 }
645 else
646 {
647 // segment and triangle are parallel
648 kTriSeg.mOrig = p0;
649 kTriSeg.mDir = TriEdge0;
650 fSqrDist = OPC_SegmentRaySqrDist(segment, kTriSeg);
651
652 kTriSeg.mDir = TriEdge1;
653 fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
654 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
655
656 kTriSeg.mOrig = p1;
657 kTriSeg.mDir = TriEdge1 - TriEdge0;
658 fSqrDist0 = OPC_SegmentRaySqrDist(segment, kTriSeg);
659 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
660
661 fSqrDist0 = OPC_PointTriangleSqrDist(rkSegOrigin, p0, p1, p2);
662 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
663
664 kPt = rkSegOrigin+rkSegDirection;
665 fSqrDist0 = OPC_PointTriangleSqrDist(kPt, p0, p1, p2);
666 if(fSqrDist0<fSqrDist) fSqrDist = fSqrDist0;
667 }
668 return fabsf(fSqrDist);
669 }
670
LSSTriOverlap(const Point & vert0,const Point & vert1,const Point & vert2)671 inline_ BOOL LSSCollider::LSSTriOverlap(const Point& vert0, const Point& vert1, const Point& vert2)
672 {
673 // Stats
674 mNbVolumePrimTests++;
675
676 float s2 = OPC_SegmentTriangleSqrDist(mSeg, vert0, vert1, vert2);
677 if(s2<mRadius2) return TRUE;
678 return FALSE;
679 }
680