1 // This file contains some code based on the code from Magic Software.
2 // That code is available under a Free Source License Agreement
3 // that can be found at http://www.magic-software.com/License/free.pdf
4
5 #include <ode/common.h>
6 #include <ode/odemath.h>
7 #include <ode/collision.h>
8 #include "collision_trimesh_internal.h"
9
10 //------------------------------------------------------------------------------
11 /**
12 @brief Finds the shortest distance squared between a point and a triangle.
13
14 @param pfSParam Barycentric coordinate of triangle at point closest to p (u)
15 @param pfTParam Barycentric coordinate of triangle at point closest to p (v)
16 @return Shortest distance squared.
17
18 The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v
19
20 Taken from:
21 Magic Software, Inc.
22 http://www.magic-software.com
23 */
SqrDistancePointTri(const dVector3 p,const dVector3 triOrigin,const dVector3 triEdge0,const dVector3 triEdge1,dReal * pfSParam,dReal * pfTParam)24 dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin,
25 const dVector3 triEdge0, const dVector3 triEdge1,
26 dReal* pfSParam, dReal* pfTParam )
27 {
28 dVector3 kDiff;
29 Vector3Subtract( triOrigin, p, kDiff );
30 dReal fA00 = dDOT( triEdge0, triEdge0 );
31 dReal fA01 = dDOT( triEdge0, triEdge1 );
32 dReal fA11 = dDOT( triEdge1, triEdge1 );
33 dReal fB0 = dDOT( kDiff, triEdge0 );
34 dReal fB1 = dDOT( kDiff, triEdge1 );
35 dReal fC = dDOT( kDiff, kDiff );
36 dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01));
37 dReal fS = fA01*fB1-fA11*fB0;
38 dReal fT = fA01*fB0-fA00*fB1;
39 dReal fSqrDist;
40
41 if ( fS + fT <= fDet )
42 {
43 if ( fS < REAL(0.0) )
44 {
45 if ( fT < REAL(0.0) ) // region 4
46 {
47 if ( fB0 < REAL(0.0) )
48 {
49 fT = REAL(0.0);
50 if ( -fB0 >= fA00 )
51 {
52 fS = REAL(1.0);
53 fSqrDist = fA00+REAL(2.0)*fB0+fC;
54 }
55 else
56 {
57 fS = -fB0/fA00;
58 fSqrDist = fB0*fS+fC;
59 }
60 }
61 else
62 {
63 fS = REAL(0.0);
64 if ( fB1 >= REAL(0.0) )
65 {
66 fT = REAL(0.0);
67 fSqrDist = fC;
68 }
69 else if ( -fB1 >= fA11 )
70 {
71 fT = REAL(1.0);
72 fSqrDist = fA11+REAL(2.0)*fB1+fC;
73 }
74 else
75 {
76 fT = -fB1/fA11;
77 fSqrDist = fB1*fT+fC;
78 }
79 }
80 }
81 else // region 3
82 {
83 fS = REAL(0.0);
84 if ( fB1 >= REAL(0.0) )
85 {
86 fT = REAL(0.0);
87 fSqrDist = fC;
88 }
89 else if ( -fB1 >= fA11 )
90 {
91 fT = REAL(1.0);
92 fSqrDist = fA11+REAL(2.0)*fB1+fC;
93 }
94 else
95 {
96 fT = -fB1/fA11;
97 fSqrDist = fB1*fT+fC;
98 }
99 }
100 }
101 else if ( fT < REAL(0.0) ) // region 5
102 {
103 fT = REAL(0.0);
104 if ( fB0 >= REAL(0.0) )
105 {
106 fS = REAL(0.0);
107 fSqrDist = fC;
108 }
109 else if ( -fB0 >= fA00 )
110 {
111 fS = REAL(1.0);
112 fSqrDist = fA00+REAL(2.0)*fB0+fC;
113 }
114 else
115 {
116 fS = -fB0/fA00;
117 fSqrDist = fB0*fS+fC;
118 }
119 }
120 else // region 0
121 {
122 // minimum at interior point
123 if ( fDet == REAL(0.0) )
124 {
125 fS = REAL(0.0);
126 fT = REAL(0.0);
127 fSqrDist = dInfinity;
128 }
129 else
130 {
131 dReal fInvDet = REAL(1.0)/fDet;
132 fS *= fInvDet;
133 fT *= fInvDet;
134 fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
135 fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
136 }
137 }
138 }
139 else
140 {
141 dReal fTmp0, fTmp1, fNumer, fDenom;
142
143 if ( fS < REAL(0.0) ) // region 2
144 {
145 fTmp0 = fA01 + fB0;
146 fTmp1 = fA11 + fB1;
147 if ( fTmp1 > fTmp0 )
148 {
149 fNumer = fTmp1 - fTmp0;
150 fDenom = fA00-REAL(2.0)*fA01+fA11;
151 if ( fNumer >= fDenom )
152 {
153 fS = REAL(1.0);
154 fT = REAL(0.0);
155 fSqrDist = fA00+REAL(2.0)*fB0+fC;
156 }
157 else
158 {
159 fS = fNumer/fDenom;
160 fT = REAL(1.0) - fS;
161 fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
162 fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
163 }
164 }
165 else
166 {
167 fS = REAL(0.0);
168 if ( fTmp1 <= REAL(0.0) )
169 {
170 fT = REAL(1.0);
171 fSqrDist = fA11+REAL(2.0)*fB1+fC;
172 }
173 else if ( fB1 >= REAL(0.0) )
174 {
175 fT = REAL(0.0);
176 fSqrDist = fC;
177 }
178 else
179 {
180 fT = -fB1/fA11;
181 fSqrDist = fB1*fT+fC;
182 }
183 }
184 }
185 else if ( fT < REAL(0.0) ) // region 6
186 {
187 fTmp0 = fA01 + fB1;
188 fTmp1 = fA00 + fB0;
189 if ( fTmp1 > fTmp0 )
190 {
191 fNumer = fTmp1 - fTmp0;
192 fDenom = fA00-REAL(2.0)*fA01+fA11;
193 if ( fNumer >= fDenom )
194 {
195 fT = REAL(1.0);
196 fS = REAL(0.0);
197 fSqrDist = fA11+REAL(2.0)*fB1+fC;
198 }
199 else
200 {
201 fT = fNumer/fDenom;
202 fS = REAL(1.0) - fT;
203 fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
204 fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
205 }
206 }
207 else
208 {
209 fT = REAL(0.0);
210 if ( fTmp1 <= REAL(0.0) )
211 {
212 fS = REAL(1.0);
213 fSqrDist = fA00+REAL(2.0)*fB0+fC;
214 }
215 else if ( fB0 >= REAL(0.0) )
216 {
217 fS = REAL(0.0);
218 fSqrDist = fC;
219 }
220 else
221 {
222 fS = -fB0/fA00;
223 fSqrDist = fB0*fS+fC;
224 }
225 }
226 }
227 else // region 1
228 {
229 fNumer = fA11 + fB1 - fA01 - fB0;
230 if ( fNumer <= REAL(0.0) )
231 {
232 fS = REAL(0.0);
233 fT = REAL(1.0);
234 fSqrDist = fA11+REAL(2.0)*fB1+fC;
235 }
236 else
237 {
238 fDenom = fA00-REAL(2.0)*fA01+fA11;
239 if ( fNumer >= fDenom )
240 {
241 fS = REAL(1.0);
242 fT = REAL(0.0);
243 fSqrDist = fA00+REAL(2.0)*fB0+fC;
244 }
245 else
246 {
247 fS = fNumer/fDenom;
248 fT = REAL(1.0) - fS;
249 fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
250 fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
251 }
252 }
253 }
254 }
255
256 if ( pfSParam )
257 *pfSParam = (float)fS;
258
259 if ( pfTParam )
260 *pfTParam = (float)fT;
261
262 return dReal(fabs(fSqrDist));
263 }
264
265 //------------------------------------------------------------------------------
266 /**
267 @brief Finds the shortest distance squared between two line segments.
268 @param pfSegP0 t value for seg1 where the shortest distance between
269 the segments exists.
270 @param pfSegP0 t value for seg2 where the shortest distance between
271 the segments exists.
272 @return Shortest distance squared.
273
274 Taken from:
275 Magic Software, Inc.
276 http://www.magic-software.com
277 */
SqrDistanceSegments(const dVector3 seg1Origin,const dVector3 seg1Direction,const dVector3 seg2Origin,const dVector3 seg2Direction,dReal * pfSegP0,dReal * pfSegP1)278 dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction,
279 const dVector3 seg2Origin, const dVector3 seg2Direction,
280 dReal* pfSegP0, dReal* pfSegP1 )
281 {
282 const dReal gs_fTolerance = 1e-05f;
283 dVector3 kDiff, kNegDiff, seg1NegDirection;
284 Vector3Subtract( seg1Origin, seg2Origin, kDiff );
285 Vector3Negate( kDiff, kNegDiff );
286 dReal fA00 = dDOT( seg1Direction, seg1Direction );
287 Vector3Negate( seg1Direction, seg1NegDirection );
288 dReal fA01 = dDOT( seg1NegDirection, seg2Direction );
289 dReal fA11 = dDOT( seg2Direction, seg2Direction );
290 dReal fB0 = dDOT( kDiff, seg1Direction );
291 dReal fC = dDOT( kDiff, kDiff );
292 dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01));
293 dReal fB1, fS, fT, fSqrDist, fTmp;
294
295 if ( fDet >= gs_fTolerance )
296 {
297 // line segments are not parallel
298 fB1 = dDOT( kNegDiff, seg2Direction );
299 fS = fA01*fB1-fA11*fB0;
300 fT = fA01*fB0-fA00*fB1;
301
302 if ( fS >= REAL(0.0) )
303 {
304 if ( fS <= fDet )
305 {
306 if ( fT >= REAL(0.0) )
307 {
308 if ( fT <= fDet ) // region 0 (interior)
309 {
310 // minimum at two interior points of 3D lines
311 dReal fInvDet = REAL(1.0)/fDet;
312 fS *= fInvDet;
313 fT *= fInvDet;
314 fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
315 fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
316 }
317 else // region 3 (side)
318 {
319 fT = REAL(1.0);
320 fTmp = fA01+fB0;
321 if ( fTmp >= REAL(0.0) )
322 {
323 fS = REAL(0.0);
324 fSqrDist = fA11+REAL(2.0)*fB1+fC;
325 }
326 else if ( -fTmp >= fA00 )
327 {
328 fS = REAL(1.0);
329 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp);
330 }
331 else
332 {
333 fS = -fTmp/fA00;
334 fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
335 }
336 }
337 }
338 else // region 7 (side)
339 {
340 fT = REAL(0.0);
341 if ( fB0 >= REAL(0.0) )
342 {
343 fS = REAL(0.0);
344 fSqrDist = fC;
345 }
346 else if ( -fB0 >= fA00 )
347 {
348 fS = REAL(1.0);
349 fSqrDist = fA00+REAL(2.0)*fB0+fC;
350 }
351 else
352 {
353 fS = -fB0/fA00;
354 fSqrDist = fB0*fS+fC;
355 }
356 }
357 }
358 else
359 {
360 if ( fT >= REAL(0.0) )
361 {
362 if ( fT <= fDet ) // region 1 (side)
363 {
364 fS = REAL(1.0);
365 fTmp = fA01+fB1;
366 if ( fTmp >= REAL(0.0) )
367 {
368 fT = REAL(0.0);
369 fSqrDist = fA00+REAL(2.0)*fB0+fC;
370 }
371 else if ( -fTmp >= fA11 )
372 {
373 fT = REAL(1.0);
374 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
375 }
376 else
377 {
378 fT = -fTmp/fA11;
379 fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
380 }
381 }
382 else // region 2 (corner)
383 {
384 fTmp = fA01+fB0;
385 if ( -fTmp <= fA00 )
386 {
387 fT = REAL(1.0);
388 if ( fTmp >= REAL(0.0) )
389 {
390 fS = REAL(0.0);
391 fSqrDist = fA11+REAL(2.0)*fB1+fC;
392 }
393 else
394 {
395 fS = -fTmp/fA00;
396 fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
397 }
398 }
399 else
400 {
401 fS = REAL(1.0);
402 fTmp = fA01+fB1;
403 if ( fTmp >= REAL(0.0) )
404 {
405 fT = REAL(0.0);
406 fSqrDist = fA00+REAL(2.0)*fB0+fC;
407 }
408 else if ( -fTmp >= fA11 )
409 {
410 fT = REAL(1.0);
411 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
412 }
413 else
414 {
415 fT = -fTmp/fA11;
416 fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
417 }
418 }
419 }
420 }
421 else // region 8 (corner)
422 {
423 if ( -fB0 < fA00 )
424 {
425 fT = REAL(0.0);
426 if ( fB0 >= REAL(0.0) )
427 {
428 fS = REAL(0.0);
429 fSqrDist = fC;
430 }
431 else
432 {
433 fS = -fB0/fA00;
434 fSqrDist = fB0*fS+fC;
435 }
436 }
437 else
438 {
439 fS = REAL(1.0);
440 fTmp = fA01+fB1;
441 if ( fTmp >= REAL(0.0) )
442 {
443 fT = REAL(0.0);
444 fSqrDist = fA00+REAL(2.0)*fB0+fC;
445 }
446 else if ( -fTmp >= fA11 )
447 {
448 fT = REAL(1.0);
449 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
450 }
451 else
452 {
453 fT = -fTmp/fA11;
454 fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
455 }
456 }
457 }
458 }
459 }
460 else
461 {
462 if ( fT >= REAL(0.0) )
463 {
464 if ( fT <= fDet ) // region 5 (side)
465 {
466 fS = REAL(0.0);
467 if ( fB1 >= REAL(0.0) )
468 {
469 fT = REAL(0.0);
470 fSqrDist = fC;
471 }
472 else if ( -fB1 >= fA11 )
473 {
474 fT = REAL(1.0);
475 fSqrDist = fA11+REAL(2.0)*fB1+fC;
476 }
477 else
478 {
479 fT = -fB1/fA11;
480 fSqrDist = fB1*fT+fC;
481 }
482 }
483 else // region 4 (corner)
484 {
485 fTmp = fA01+fB0;
486 if ( fTmp < REAL(0.0) )
487 {
488 fT = REAL(1.0);
489 if ( -fTmp >= fA00 )
490 {
491 fS = REAL(1.0);
492 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp);
493 }
494 else
495 {
496 fS = -fTmp/fA00;
497 fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
498 }
499 }
500 else
501 {
502 fS = REAL(0.0);
503 if ( fB1 >= REAL(0.0) )
504 {
505 fT = REAL(0.0);
506 fSqrDist = fC;
507 }
508 else if ( -fB1 >= fA11 )
509 {
510 fT = REAL(1.0);
511 fSqrDist = fA11+REAL(2.0)*fB1+fC;
512 }
513 else
514 {
515 fT = -fB1/fA11;
516 fSqrDist = fB1*fT+fC;
517 }
518 }
519 }
520 }
521 else // region 6 (corner)
522 {
523 if ( fB0 < REAL(0.0) )
524 {
525 fT = REAL(0.0);
526 if ( -fB0 >= fA00 )
527 {
528 fS = REAL(1.0);
529 fSqrDist = fA00+REAL(2.0)*fB0+fC;
530 }
531 else
532 {
533 fS = -fB0/fA00;
534 fSqrDist = fB0*fS+fC;
535 }
536 }
537 else
538 {
539 fS = REAL(0.0);
540 if ( fB1 >= REAL(0.0) )
541 {
542 fT = REAL(0.0);
543 fSqrDist = fC;
544 }
545 else if ( -fB1 >= fA11 )
546 {
547 fT = REAL(1.0);
548 fSqrDist = fA11+REAL(2.0)*fB1+fC;
549 }
550 else
551 {
552 fT = -fB1/fA11;
553 fSqrDist = fB1*fT+fC;
554 }
555 }
556 }
557 }
558 }
559 else
560 {
561 // line segments are parallel
562 if ( fA01 > REAL(0.0) )
563 {
564 // direction vectors form an obtuse angle
565 if ( fB0 >= REAL(0.0) )
566 {
567 fS = REAL(0.0);
568 fT = REAL(0.0);
569 fSqrDist = fC;
570 }
571 else if ( -fB0 <= fA00 )
572 {
573 fS = -fB0/fA00;
574 fT = REAL(0.0);
575 fSqrDist = fB0*fS+fC;
576 }
577 else
578 {
579 //fB1 = -kDiff % seg2.m;
580 fB1 = dDOT( kNegDiff, seg2Direction );
581 fS = REAL(1.0);
582 fTmp = fA00+fB0;
583 if ( -fTmp >= fA01 )
584 {
585 fT = REAL(1.0);
586 fSqrDist = fA00+fA11+fC+REAL(2.0)*(fA01+fB0+fB1);
587 }
588 else
589 {
590 fT = -fTmp/fA01;
591 fSqrDist = fA00+REAL(2.0)*fB0+fC+fT*(fA11*fT+REAL(2.0)*(fA01+fB1));
592 }
593 }
594 }
595 else
596 {
597 // direction vectors form an acute angle
598 if ( -fB0 >= fA00 )
599 {
600 fS = REAL(1.0);
601 fT = REAL(0.0);
602 fSqrDist = fA00+REAL(2.0)*fB0+fC;
603 }
604 else if ( fB0 <= REAL(0.0) )
605 {
606 fS = -fB0/fA00;
607 fT = REAL(0.0);
608 fSqrDist = fB0*fS+fC;
609 }
610 else
611 {
612 fB1 = dDOT( kNegDiff, seg2Direction );
613 fS = REAL(0.0);
614 if ( fB0 >= -fA01 )
615 {
616 fT = REAL(1.0);
617 fSqrDist = fA11+REAL(2.0)*fB1+fC;
618 }
619 else
620 {
621 fT = -fB0/fA01;
622 fSqrDist = fC+fT*(REAL(2.0)*fB1+fA11*fT);
623 }
624 }
625 }
626 }
627
628 if ( pfSegP0 )
629 *pfSegP0 = fS;
630
631 if ( pfSegP1 )
632 *pfSegP1 = fT;
633
634 return dReal(fabs(fSqrDist));
635 }
636
637 //------------------------------------------------------------------------------
638 /**
639 @brief Finds the shortest distance squared between a line segment and
640 a triangle.
641
642 @param pfSegP t value for the line segment where the shortest distance between
643 the segment and the triangle occurs.
644 So the point along the segment that is the shortest distance
645 away from the triangle can be obtained by (seg.end - seg.start) * t.
646 @param pfTriP0 Barycentric coordinate of triangle at point closest to seg (u)
647 @param pfTriP1 Barycentric coordinate of triangle at point closest to seg (v)
648 @return Shortest distance squared.
649
650 The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v
651
652 Taken from:
653 Magic Software, Inc.
654 http://www.magic-software.com
655 */
SqrDistanceSegTri(const dVector3 segOrigin,const dVector3 segEnd,const dVector3 triOrigin,const dVector3 triEdge0,const dVector3 triEdge1,dReal * pfSegP,dReal * pfTriP0,dReal * pfTriP1)656 dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd,
657 const dVector3 triOrigin,
658 const dVector3 triEdge0, const dVector3 triEdge1,
659 dReal* pfSegP, dReal* pfTriP0, dReal* pfTriP1 )
660 {
661 const dReal gs_fTolerance = 1e-06f;
662 dVector3 segDirection, segNegDirection, kDiff, kNegDiff;
663 Vector3Subtract( segEnd, segOrigin, segDirection );
664 Vector3Negate( segDirection, segNegDirection );
665 Vector3Subtract( triOrigin, segOrigin, kDiff );
666 Vector3Negate( kDiff, kNegDiff );
667 dReal fA00 = dDOT( segDirection, segDirection );
668 dReal fA01 = dDOT( segNegDirection, triEdge0 );
669 dReal fA02 = dDOT( segNegDirection, triEdge1 );
670 dReal fA11 = dDOT( triEdge0, triEdge0 );
671 dReal fA12 = dDOT( triEdge0, triEdge1 );
672 dReal fA22 = dDOT( triEdge1, triEdge1 );
673 dReal fB0 = dDOT( kNegDiff, segDirection );
674 dReal fB1 = dDOT( kDiff, triEdge0 );
675 dReal fB2 = dDOT( kDiff, triEdge1 );
676
677 dVector3 kTriSegOrigin, kTriSegDirection, kPt;
678 dReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;
679
680 // Set up for a relative error test on the angle between ray direction
681 // and triangle normal to determine parallel/nonparallel status.
682 dVector3 kN;
683 dCROSS( kN, =, triEdge0, triEdge1 );
684 dReal fNSqrLen = dDOT( kN, kN );
685 dReal fDot = dDOT( segDirection, kN );
686 bool bNotParallel = (fDot*fDot >= gs_fTolerance*fA00*fNSqrLen);
687
688 if ( bNotParallel )
689 {
690 dReal fCof00 = fA11*fA22-fA12*fA12;
691 dReal fCof01 = fA02*fA12-fA01*fA22;
692 dReal fCof02 = fA01*fA12-fA02*fA11;
693 dReal fCof11 = fA00*fA22-fA02*fA02;
694 dReal fCof12 = fA02*fA01-fA00*fA12;
695 dReal fCof22 = fA00*fA11-fA01*fA01;
696 dReal fInvDet = REAL(1.0)/(fA00*fCof00+fA01*fCof01+fA02*fCof02);
697 dReal fRhs0 = -fB0*fInvDet;
698 dReal fRhs1 = -fB1*fInvDet;
699 dReal fRhs2 = -fB2*fInvDet;
700
701 fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
702 fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
703 fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;
704
705 if ( fR < REAL(0.0) )
706 {
707 if ( fS+fT <= REAL(1.0) )
708 {
709 if ( fS < REAL(0.0) )
710 {
711 if ( fT < REAL(0.0) ) // region 4m
712 {
713 // min on face s=0 or t=0 or r=0
714 Vector3Copy( triOrigin, kTriSegOrigin );
715 Vector3Copy( triEdge1, kTriSegDirection );
716 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
717 kTriSegOrigin, kTriSegDirection,
718 &fR, &fT );
719 fS = REAL(0.0);
720 Vector3Copy( triOrigin, kTriSegOrigin );
721 Vector3Copy( triEdge0, kTriSegDirection );
722 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
723 kTriSegOrigin, kTriSegDirection,
724 &fR0, &fS0 );
725 fT0 = REAL(0.0);
726 if ( fSqrDist0 < fSqrDist )
727 {
728 fSqrDist = fSqrDist0;
729 fR = fR0;
730 fS = fS0;
731 fT = fT0;
732 }
733 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
734 &fS0, &fT0 );
735 fR0 = REAL(0.0);
736 if ( fSqrDist0 < fSqrDist )
737 {
738 fSqrDist = fSqrDist0;
739 fR = fR0;
740 fS = fS0;
741 fT = fT0;
742 }
743 }
744 else // region 3m
745 {
746 // min on face s=0 or r=0
747 Vector3Copy( triOrigin, kTriSegOrigin );
748 Vector3Copy( triEdge1, kTriSegDirection );
749 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
750 kTriSegOrigin, kTriSegDirection,
751 &fR,&fT );
752 fS = REAL(0.0);
753 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
754 &fS0, &fT0 );
755 fR0 = REAL(0.0);
756 if ( fSqrDist0 < fSqrDist )
757 {
758 fSqrDist = fSqrDist0;
759 fR = fR0;
760 fS = fS0;
761 fT = fT0;
762 }
763 }
764 }
765 else if ( fT < REAL(0.0) ) // region 5m
766 {
767 // min on face t=0 or r=0
768 Vector3Copy( triOrigin, kTriSegOrigin );
769 Vector3Copy( triEdge0, kTriSegDirection );
770 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
771 kTriSegOrigin, kTriSegDirection,
772 &fR, &fS );
773 fT = REAL(0.0);
774 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
775 &fS0, &fT0 );
776 fR0 = REAL(0.0);
777 if ( fSqrDist0 < fSqrDist )
778 {
779 fSqrDist = fSqrDist0;
780 fR = fR0;
781 fS = fS0;
782 fT = fT0;
783 }
784 }
785 else // region 0m
786 {
787 // min on face r=0
788 fSqrDist = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
789 &fS, &fT );
790 fR = REAL(0.0);
791 }
792 }
793 else
794 {
795 if ( fS < REAL(0.0) ) // region 2m
796 {
797 // min on face s=0 or s+t=1 or r=0
798 Vector3Copy( triOrigin, kTriSegOrigin );
799 Vector3Copy( triEdge1, kTriSegDirection );
800 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
801 kTriSegOrigin, kTriSegDirection,
802 &fR, &fT );
803 fS = REAL(0.0);
804 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
805 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
806 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
807 kTriSegOrigin, kTriSegDirection,
808 &fR0, &fT0 );
809 fS0 = REAL(1.0) - fT0;
810 if ( fSqrDist0 < fSqrDist )
811 {
812 fSqrDist = fSqrDist0;
813 fR = fR0;
814 fS = fS0;
815 fT = fT0;
816 }
817 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
818 &fS0, &fT0 );
819 fR0 = REAL(0.0);
820 if ( fSqrDist0 < fSqrDist )
821 {
822 fSqrDist = fSqrDist0;
823 fR = fR0;
824 fS = fS0;
825 fT = fT0;
826 }
827 }
828 else if ( fT < REAL(0.0) ) // region 6m
829 {
830 // min on face t=0 or s+t=1 or r=0
831 Vector3Copy( triOrigin, kTriSegOrigin );
832 Vector3Copy( triEdge0, kTriSegDirection );
833 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
834 kTriSegOrigin, kTriSegDirection,
835 &fR, &fS );
836 fT = REAL(0.0);
837 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
838 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
839 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
840 kTriSegOrigin, kTriSegDirection,
841 &fR0, &fT0 );
842 fS0 = REAL(1.0) - fT0;
843 if ( fSqrDist0 < fSqrDist )
844 {
845 fSqrDist = fSqrDist0;
846 fR = fR0;
847 fS = fS0;
848 fT = fT0;
849 }
850 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
851 &fS0, &fT0 );
852 fR0 = REAL(0.0);
853 if ( fSqrDist0 < fSqrDist )
854 {
855 fSqrDist = fSqrDist0;
856 fR = fR0;
857 fS = fS0;
858 fT = fT0;
859 }
860 }
861 else // region 1m
862 {
863 // min on face s+t=1 or r=0
864 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
865 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
866 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
867 kTriSegOrigin, kTriSegDirection,
868 &fR, &fT );
869 fS = REAL(1.0) - fT;
870 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
871 &fS0, &fT0 );
872 fR0 = REAL(0.0);
873 if ( fSqrDist0 < fSqrDist )
874 {
875 fSqrDist = fSqrDist0;
876 fR = fR0;
877 fS = fS0;
878 fT = fT0;
879 }
880 }
881 }
882 }
883 else if ( fR <= REAL(1.0) )
884 {
885 if ( fS+fT <= REAL(1.0) )
886 {
887 if ( fS < REAL(0.0) )
888 {
889 if ( fT < REAL(0.0) ) // region 4
890 {
891 // min on face s=0 or t=0
892 Vector3Copy( triOrigin, kTriSegOrigin );
893 Vector3Copy( triEdge1, kTriSegDirection );
894 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
895 kTriSegOrigin, kTriSegDirection,
896 &fR, &fT );
897 fS = REAL(0.0);
898 Vector3Copy( triOrigin, kTriSegOrigin );
899 Vector3Copy( triEdge0, kTriSegDirection );
900 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
901 kTriSegOrigin, kTriSegDirection,
902 &fR0, &fS0 );
903 fT0 = REAL(0.0);
904 if ( fSqrDist0 < fSqrDist )
905 {
906 fSqrDist = fSqrDist0;
907 fR = fR0;
908 fS = fS0;
909 fT = fT0;
910 }
911 }
912 else // region 3
913 {
914 // min on face s=0
915 Vector3Copy( triOrigin, kTriSegOrigin );
916 Vector3Copy( triEdge1, kTriSegDirection );
917 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
918 kTriSegOrigin, kTriSegDirection,
919 &fR, &fT );
920 fS = REAL(0.0);
921 }
922 }
923 else if ( fT < REAL(0.0) ) // region 5
924 {
925 // min on face t=0
926 Vector3Copy( triOrigin, kTriSegOrigin );
927 Vector3Copy( triEdge0, kTriSegDirection );
928 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
929 kTriSegOrigin, kTriSegDirection,
930 &fR, &fS );
931 fT = REAL(0.0);
932 }
933 else // region 0
934 {
935 // global minimum is interior, done
936 fSqrDist = REAL(0.0);
937 }
938 }
939 else
940 {
941 if ( fS < REAL(0.0) ) // region 2
942 {
943 // min on face s=0 or s+t=1
944 Vector3Copy( triOrigin, kTriSegOrigin );
945 Vector3Copy( triEdge1, kTriSegDirection );
946 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
947 kTriSegOrigin, kTriSegDirection,
948 &fR, &fT );
949 fS = REAL(0.0);
950 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
951 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
952 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
953 kTriSegOrigin, kTriSegDirection,
954 &fR0, &fT0 );
955 fS0 = REAL(1.0) - fT0;
956 if ( fSqrDist0 < fSqrDist )
957 {
958 fSqrDist = fSqrDist0;
959 fR = fR0;
960 fS = fS0;
961 fT = fT0;
962 }
963 }
964 else if ( fT < REAL(0.0) ) // region 6
965 {
966 // min on face t=0 or s+t=1
967 Vector3Copy( triOrigin, kTriSegOrigin );
968 Vector3Copy( triEdge0, kTriSegDirection );
969 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
970 kTriSegOrigin, kTriSegDirection,
971 &fR, &fS );
972 fT = REAL(0.0);
973 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
974 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
975 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
976 kTriSegOrigin, kTriSegDirection,
977 &fR0, &fT0 );
978 fS0 = REAL(1.0) - fT0;
979 if ( fSqrDist0 < fSqrDist )
980 {
981 fSqrDist = fSqrDist0;
982 fR = fR0;
983 fS = fS0;
984 fT = fT0;
985 }
986 }
987 else // region 1
988 {
989 // min on face s+t=1
990 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
991 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
992 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
993 kTriSegOrigin, kTriSegDirection,
994 &fR, &fT );
995 fS = REAL(1.0) - fT;
996 }
997 }
998 }
999 else // fR > 1
1000 {
1001 if ( fS+fT <= REAL(1.0) )
1002 {
1003 if ( fS < REAL(0.0) )
1004 {
1005 if ( fT < REAL(0.0) ) // region 4p
1006 {
1007 // min on face s=0 or t=0 or r=1
1008 Vector3Copy( triOrigin, kTriSegOrigin );
1009 Vector3Copy( triEdge1, kTriSegDirection );
1010 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1011 kTriSegOrigin, kTriSegDirection,
1012 &fR, &fT );
1013 fS = REAL(0.0);
1014 Vector3Copy( triOrigin, kTriSegOrigin );
1015 Vector3Copy( triEdge0, kTriSegDirection );
1016 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1017 kTriSegOrigin, kTriSegDirection,
1018 &fR0, &fS0 );
1019 fT0 = REAL(0.0);
1020 if ( fSqrDist0 < fSqrDist )
1021 {
1022 fSqrDist = fSqrDist0;
1023 fR = fR0;
1024 fS = fS0;
1025 fT = fT0;
1026 }
1027 Vector3Add( segOrigin, segDirection, kPt );
1028 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1029 &fS0, &fT0 );
1030 fR0 = REAL(1.0);
1031 if ( fSqrDist0 < fSqrDist )
1032 {
1033 fSqrDist = fSqrDist0;
1034 fR = fR0;
1035 fS = fS0;
1036 fT = fT0;
1037 }
1038 }
1039 else // region 3p
1040 {
1041 // min on face s=0 or r=1
1042 Vector3Copy( triOrigin, kTriSegOrigin );
1043 Vector3Copy( triEdge1, kTriSegDirection );
1044 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1045 kTriSegOrigin, kTriSegDirection,
1046 &fR, &fT );
1047 fS = REAL(0.0);
1048 Vector3Add( segOrigin, segDirection, kPt );
1049 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1050 &fS0, &fT0 );
1051 fR0 = REAL(1.0);
1052 if ( fSqrDist0 < fSqrDist )
1053 {
1054 fSqrDist = fSqrDist0;
1055 fR = fR0;
1056 fS = fS0;
1057 fT = fT0;
1058 }
1059 }
1060 }
1061 else if ( fT < REAL(0.0) ) // region 5p
1062 {
1063 // min on face t=0 or r=1
1064 Vector3Copy( triOrigin, kTriSegOrigin );
1065 Vector3Copy( triEdge0, kTriSegDirection );
1066 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1067 kTriSegOrigin, kTriSegDirection,
1068 &fR, &fS );
1069 fT = REAL(0.0);
1070 Vector3Add( segOrigin, segDirection, kPt );
1071 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1072 &fS0, &fT0 );
1073 fR0 = REAL(1.0);
1074 if ( fSqrDist0 < fSqrDist )
1075 {
1076 fSqrDist = fSqrDist0;
1077 fR = fR0;
1078 fS = fS0;
1079 fT = fT0;
1080 }
1081 }
1082 else // region 0p
1083 {
1084 // min face on r=1
1085 Vector3Add( segOrigin, segDirection, kPt );
1086 fSqrDist = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1087 &fS, &fT );
1088 fR = REAL(1.0);
1089 }
1090 }
1091 else
1092 {
1093 if ( fS < REAL(0.0) ) // region 2p
1094 {
1095 // min on face s=0 or s+t=1 or r=1
1096 Vector3Copy( triOrigin, kTriSegOrigin );
1097 Vector3Copy( triEdge1, kTriSegDirection );
1098 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1099 kTriSegOrigin, kTriSegDirection,
1100 &fR, &fT );
1101 fS = REAL(0.0);
1102 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1103 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1104 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1105 kTriSegOrigin, kTriSegDirection,
1106 &fR0, &fT0 );
1107 fS0 = REAL(1.0) - fT0;
1108 if ( fSqrDist0 < fSqrDist )
1109 {
1110 fSqrDist = fSqrDist0;
1111 fR = fR0;
1112 fS = fS0;
1113 fT = fT0;
1114 }
1115 Vector3Add( segOrigin, segDirection, kPt );
1116 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1117 &fS0, &fT0 );
1118 fR0 = REAL(1.0);
1119 if ( fSqrDist0 < fSqrDist )
1120 {
1121 fSqrDist = fSqrDist0;
1122 fR = fR0;
1123 fS = fS0;
1124 fT = fT0;
1125 }
1126 }
1127 else if ( fT < REAL(0.0) ) // region 6p
1128 {
1129 // min on face t=0 or s+t=1 or r=1
1130 Vector3Copy( triOrigin, kTriSegOrigin );
1131 Vector3Copy( triEdge0, kTriSegDirection );
1132 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1133 kTriSegOrigin, kTriSegDirection,
1134 &fR, &fS );
1135 fT = REAL(0.0);
1136 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1137 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1138 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1139 kTriSegOrigin, kTriSegDirection,
1140 &fR0, &fT0 );
1141 fS0 = REAL(1.0) - fT0;
1142 if ( fSqrDist0 < fSqrDist )
1143 {
1144 fSqrDist = fSqrDist0;
1145 fR = fR0;
1146 fS = fS0;
1147 fT = fT0;
1148 }
1149 Vector3Add( segOrigin, segDirection, kPt );
1150 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1151 &fS0, &fT0 );
1152 fR0 = REAL(1.0);
1153 if ( fSqrDist0 < fSqrDist )
1154 {
1155 fSqrDist = fSqrDist0;
1156 fR = fR0;
1157 fS = fS0;
1158 fT = fT0;
1159 }
1160 }
1161 else // region 1p
1162 {
1163 // min on face s+t=1 or r=1
1164 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1165 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1166 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1167 kTriSegOrigin, kTriSegDirection,
1168 &fR, &fT );
1169 fS = REAL(1.0) - fT;
1170 Vector3Add( segOrigin, segDirection, kPt );
1171 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1172 &fS0, &fT0 );
1173 fR0 = REAL(1.0);
1174 if ( fSqrDist0 < fSqrDist )
1175 {
1176 fSqrDist = fSqrDist0;
1177 fR = fR0;
1178 fS = fS0;
1179 fT = fT0;
1180 }
1181 }
1182 }
1183 }
1184 }
1185 else
1186 {
1187 // segment and triangle are parallel
1188 Vector3Copy( triOrigin, kTriSegOrigin );
1189 Vector3Copy( triEdge0, kTriSegDirection );
1190 fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
1191 kTriSegOrigin, kTriSegDirection, &fR, &fS );
1192 fT = REAL(0.0);
1193
1194 Vector3Copy( triEdge1, kTriSegDirection );
1195 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1196 kTriSegOrigin, kTriSegDirection,
1197 &fR0, &fT0 );
1198 fS0 = REAL(0.0);
1199 if ( fSqrDist0 < fSqrDist )
1200 {
1201 fSqrDist = fSqrDist0;
1202 fR = fR0;
1203 fS = fS0;
1204 fT = fT0;
1205 }
1206
1207 Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
1208 Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
1209 fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
1210 kTriSegOrigin, kTriSegDirection, &fR0, &fT0 );
1211 fS0 = REAL(1.0) - fT0;
1212 if ( fSqrDist0 < fSqrDist )
1213 {
1214 fSqrDist = fSqrDist0;
1215 fR = fR0;
1216 fS = fS0;
1217 fT = fT0;
1218 }
1219
1220 fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
1221 &fS0, &fT0 );
1222 fR0 = REAL(0.0);
1223 if ( fSqrDist0 < fSqrDist )
1224 {
1225 fSqrDist = fSqrDist0;
1226 fR = fR0;
1227 fS = fS0;
1228 fT = fT0;
1229 }
1230
1231 Vector3Add( segOrigin, segDirection, kPt );
1232 fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
1233 &fS0, &fT0 );
1234 fR0 = REAL(1.0);
1235 if ( fSqrDist0 < fSqrDist )
1236 {
1237 fSqrDist = fSqrDist0;
1238 fR = fR0;
1239 fS = fS0;
1240 fT = fT0;
1241 }
1242 }
1243
1244 if ( pfSegP )
1245 *pfSegP = fR;
1246
1247 if ( pfTriP0 )
1248 *pfTriP0 = fS;
1249
1250 if ( pfTriP1 )
1251 *pfTriP1 = fT;
1252
1253 return fSqrDist;
1254 }
1255