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