1 /**************************************************************************\
2  * Copyright (c) Kongsberg Oil & Gas Technologies AS
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are
7  * met:
8  *
9  * Redistributions of source code must retain the above copyright notice,
10  * this list of conditions and the following disclaimer.
11  *
12  * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  *
16  * Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
24  * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
25  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
26  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
27  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
28  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
29  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 \**************************************************************************/
32 
33 /*!
34   \class SbTri3f collision/SbTri3f.h
35   \brief A class that at this point in time has one purpose - figuring out
36   if two triangles intersect each other.
37 
38   \ingroup base
39 
40   This class is so limited in functionality that it is not included in the
41   public Coin API for now.
42 
43   The internals will probably be changed as well, as the a, b, c
44   representation isn't very convenient for linear algebra purposes.  But
45   as a public base class, the internal representation should be fixed, and
46   made part of the private section of the public header.
47 
48   \since 2002-10-22
49 */
50 
51 // FIXME: clean up the code -- there's lots of stuff that has been
52 // commented out without any explanation, for instance. 20030603 mortene.
53 
54 #include <cassert>
55 #include <cfloat>
56 
57 #include <Inventor/errors/SoDebugError.h>
58 #include <Inventor/SbPlane.h>
59 #include <Inventor/SbLine.h>
60 #include <Inventor/SbBox3f.h>
61 
62 #include "SbTri3f.h"
63 
64 // Here's an idea for an alternate approach for this class:
65 //
66 // Let's say the triangle is defined as (0,0,0), (1,0,0), (1,1,0)
67 // transformed through a transformation matrix.
68 //
69 // With this representation, a triangle would take the 16 floats of
70 // an SbMatrix instead of 9 for the vertex positions.
71 //
72 // Finding the normal of the triangle is a multVecMatrix() call
73 // Finding a, b, and c are multVecMatrix() calls
74 // Finding the area is a multVecMatrix() and a getLength() call
75 //
76 // Doing intersection testing would be done by transforming one primitive
77 // into the local coordinate system of the triangle (by using the inverse
78 // matrix).  Given the simplicity of the a, b, and c coordinates of the
79 // localspace triangle, the intersection testing ought to be a lot more
80 // trivial to perform than in fully flexible 3D space.  Especially the
81 // case of triangles in the same plane...
82 
83 #define SBTRI_DEBUG 0
84 
85 // *************************************************************************
86 
87 class SbTri3fP {
88 public:
SbTri3fP(void)89   SbTri3fP(void) {}
SbTri3fP(SbTri3fP * t)90   SbTri3fP(SbTri3fP * t)
91     : a(t->a), b(t->b), c(t->c) {}
SbTri3fP(const SbVec3f & na,const SbVec3f & nb,const SbVec3f & nc)92   SbTri3fP(const SbVec3f & na, const SbVec3f & nb, const SbVec3f & nc)
93     : a(na), b(nb), c(nc)
94   {
95     // FIXME: fix IDAction so this assert doesn't hit. 20030328 mortene.
96     assert(a != b && a != c && b != c);
97   }
98 
99   SbVec3f a;
100   SbVec3f b;
101   SbVec3f c;
102 };
103 
104 // *************************************************************************
105 
106 #define PRIVATE(obj) ((obj)->pimpl)
107 
SbTri3f(void)108 SbTri3f::SbTri3f(void)
109   : pimpl(new SbTri3fP)
110 {
111 }
112 
SbTri3f(const SbTri3f & t)113 SbTri3f::SbTri3f(const SbTri3f & t)
114   : pimpl(new SbTri3fP(PRIVATE(&t)))
115 {
116 }
117 
SbTri3f(const SbVec3f & a,const SbVec3f & b,const SbVec3f & c)118 SbTri3f::SbTri3f(const SbVec3f & a, const SbVec3f & b, const SbVec3f & c)
119   : pimpl(new SbTri3fP(a, b, c))
120 {
121 }
122 
~SbTri3f(void)123 SbTri3f::~SbTri3f(void)
124 {
125   delete PRIVATE(this);
126 }
127 
128 SbTri3f &
setValue(const SbTri3f & t)129 SbTri3f::setValue(const SbTri3f & t)
130 {
131   PRIVATE(this)->a = PRIVATE(&t)->a;
132   PRIVATE(this)->b = PRIVATE(&t)->b;
133   PRIVATE(this)->c = PRIVATE(&t)->c;
134   assert(PRIVATE(this)->a != PRIVATE(this)->b && PRIVATE(this)->a != PRIVATE(this)->c && PRIVATE(this)->b != PRIVATE(this)->c);
135   return *this;
136 }
137 
138 SbTri3f &
setValue(const SbVec3f & a,const SbVec3f & b,const SbVec3f & c)139 SbTri3f::setValue(const SbVec3f & a, const SbVec3f & b, const SbVec3f & c)
140 {
141   assert(a != b && a != c && b != c);
142   PRIVATE(this)->a = a;
143   PRIVATE(this)->b = b;
144   PRIVATE(this)->c = c;
145   return *this;
146 }
147 
148 void
getValue(SbTri3f & t) const149 SbTri3f::getValue(SbTri3f & t) const
150 {
151   PRIVATE(&t)->a = PRIVATE(this)->a;
152   PRIVATE(&t)->b = PRIVATE(this)->b;
153   PRIVATE(&t)->c = PRIVATE(this)->c;
154 }
155 
156 void
getValue(SbVec3f & a,SbVec3f & b,SbVec3f & c) const157 SbTri3f::getValue(SbVec3f & a, SbVec3f & b, SbVec3f & c) const
158 {
159   a = PRIVATE(this)->a;
160   b = PRIVATE(this)->b;
161   c = PRIVATE(this)->c;
162 }
163 
164 SbTri3f &
operator =(const SbTri3f & t)165 SbTri3f::operator = (const SbTri3f & t)
166 {
167   PRIVATE(this)->a = PRIVATE(&t)->a;
168   PRIVATE(this)->b = PRIVATE(&t)->b;
169   PRIVATE(this)->c = PRIVATE(&t)->c;
170   return *this;
171 }
172 
173 // *************************************************************************
174 
175 SbBool
intersect(const SbTri3f & t) const176 SbTri3f::intersect(const SbTri3f & t) const
177 {
178   // FIXME: remove all "programming logic" error messages and asserts from
179   // this function when it's verified that those paths can't be taken.
180 
181   SbVec3f a1(PRIVATE(this)->a);
182   SbVec3f b1(PRIVATE(this)->b);
183   SbVec3f c1(PRIVATE(this)->c);
184   SbPlane plane1(a1, b1, c1);
185 
186   SbVec3f a2(PRIVATE(&t)->a);
187   SbVec3f b2(PRIVATE(&t)->b);
188   SbVec3f c2(PRIVATE(&t)->c);
189   SbPlane plane2(a2, b2, c2);
190 
191   // FIXME: can ((n1 == -n2) && (d1 == -d2)) really happen?
192   if (SBTRI_DEBUG &&
193        (plane1.getNormal() == -plane2.getNormal()) &&
194        (plane1.getDistanceFromOrigin() == -plane2.getDistanceFromOrigin())) {
195     SoDebugError::post("SbTri3f::intersect", "The (n1 == -n2 && d1 == -d2) case happened");
196   }
197   if (plane1.getNormal() == plane2.getNormal()) {
198     // fprintf(stderr, "normals are equal\n");
199     if (plane1.getDistanceFromOrigin() != plane2.getDistanceFromOrigin())
200       return FALSE; // parallel planes
201     // we work around coplanar intersection testing by making it a case of
202     // biplanar intersection testing.
203 
204     int vertex = 1;
205     float distance = a1.sqrLength();
206     float d;
207     if ((d = b1.sqrLength()) > distance) {
208       distance = d;
209       vertex = 2;
210     } else if ((d = c1.sqrLength()) > distance) {
211       distance = d;
212       vertex = 3;
213     } else if ((d = a2.sqrLength()) > distance) {
214       distance = d;
215       vertex = 4;
216     } else if ((d = b2.sqrLength()) > distance) {
217       distance = d;
218       vertex = 5;
219     } else if ((d = c2.sqrLength()) > distance) {
220       distance = d;
221       vertex = 6;
222     }
223     switch (vertex) {
224     case 1:
225       break;
226     case 2:
227       do { SbVec3f temp(a1); a1 = b1; b1 = c1; c1 = temp; } while (FALSE);
228       break;
229     case 3:
230       do { SbVec3f temp(a1); a1 = c1; c1 = b1; b1 = temp; } while (FALSE);
231       break;
232     case 4:
233       do { SbVec3f temp(a1); a1 = a2; a2 = temp; } while (FALSE);
234       do { SbVec3f temp(b1); b1 = b2; b2 = temp; } while (FALSE);
235       do { SbVec3f temp(c1); c1 = c2; c2 = temp; } while (FALSE);
236       break;
237     case 5:
238       do { SbVec3f temp(a1); a1 = b2; b2 = temp; } while (FALSE);
239       do { SbVec3f temp(b1); b1 = c2; c2 = temp; } while (FALSE);
240       do { SbVec3f temp(c1); c1 = a2; a2 = temp; } while (FALSE);
241       break;
242     case 6:
243       do { SbVec3f temp(a1); a1 = c2; c2 = temp; } while (FALSE);
244       do { SbVec3f temp(b1); b1 = a2; a2 = temp; } while (FALSE);
245       do { SbVec3f temp(c1); c1 = b2; b2 = temp; } while (FALSE);
246       break;
247     }
248     vertex = 1;
249     distance = (a2-a1).sqrLength();
250     if ((d = (b2-a1).sqrLength()) > distance) {
251       distance = d;
252       vertex = 2;
253     } else if ((d = (c2-a1).sqrLength()) > distance) {
254       distance = d;
255       vertex = 3;
256     }
257     switch (vertex) {
258     case 1:
259       break;
260     case 2:
261       do { SbVec3f temp(a2); a2 = b2; b2 = b2; c2 = temp; } while (FALSE);
262       break;
263     case 3:
264       do { SbVec3f temp(a2); a2 = c2; c2 = b2; b2 = temp; } while (FALSE);
265       break;
266     }
267 
268     // FIXME: I'm not confident we've actually found the two vertices that should
269     // be lifted up at this point.  I can think of cases that will give false
270     // negatives.  20021024 larsa
271     a1 = a1 + plane1.getNormal();
272     a2 = a2 + plane1.getNormal();
273     // regenerate planes
274     plane1 = SbPlane(a1, b1, c1);
275     plane2 = SbPlane(a2, b2, c2);
276   // } else {
277     // fprintf(stderr, "normals are different\n");
278   }
279 
280   // set up point a on one side, and b and c on the other
281 
282   const SbBool a1hs = plane2.isInHalfSpace(a1);
283   const SbBool b1hs = plane2.isInHalfSpace(b1);
284   const SbBool c1hs = plane2.isInHalfSpace(c1);
285   if ((a1hs == b1hs) && (a1hs == c1hs)) {
286     // no intersection
287     return FALSE;
288   } else if (a1hs == c1hs) { // b is in other halfspace
289     SbVec3f temp(a1); a1 = b1; b1 = c1; c1 = temp;
290   } else if (a1hs == b1hs) { // c is in other halfspace
291     SbVec3f temp(a1); a1 = c1; c1 = b1; b1 = temp;
292   }
293 
294   const SbBool a2hs = plane1.isInHalfSpace(a2);
295   const SbBool b2hs = plane1.isInHalfSpace(b2);
296   const SbBool c2hs = plane1.isInHalfSpace(c2);
297   if ((a2hs == b2hs) && (a2hs == c2hs)) {
298     // no intersection
299     return FALSE;
300   } else if (a2hs == c2hs) { // b is in other halfspace
301     SbVec3f temp(a2); a2 = b2; b2 = c2; c2 = temp;
302   } else if (a2hs == b2hs) { // c is in other halfspace
303     SbVec3f temp(a2); a2 = c2; c2 = b2; b2 = temp;
304   }
305 
306   // find intersection points on line for triangles
307   SbVec3f p11, p12;
308   if (!plane2.intersect(SbLine(a1, b1), p11)) {
309     // should really never happen
310     if (SBTRI_DEBUG) {
311       SoDebugError::post("SbTri3f::intersect", "programming logic error 1");
312       SoDebugError::post("-", "SbVec3f a1(%g, %g, %g);", a1[0], a1[1], a1[2]);
313       SoDebugError::post("-", "SbVec3f b1(%g, %g, %g);", b1[0], b1[1], b1[2]);
314       SoDebugError::post("-", "SbVec3f c1(%g, %g, %g);", c1[0], c1[1], c1[2]);
315       SoDebugError::post("-", "SbVec3f a2(%g, %g, %g);", a2[0], a2[1], a2[2]);
316       SoDebugError::post("-", "SbVec3f b2(%g, %g, %g);", b2[0], b2[1], b2[2]);
317       SoDebugError::post("-", "SbVec3f c2(%g, %g, %g);", c2[0], c2[1], c2[2]);
318       assert(0);
319     }
320     return FALSE;
321   }
322   if (!plane2.intersect(SbLine(a1, c1), p12)) {
323     // should never happen
324     if (SBTRI_DEBUG) {
325       SoDebugError::post("SbTri3f::intersect", "programming logic error 2");
326       SoDebugError::post("-", "SbVec3f a1(%g, %g, %g);", a1[0], a1[1], a1[2]);
327       SoDebugError::post("-", "SbVec3f b1(%g, %g, %g);", b1[0], b1[1], b1[2]);
328       SoDebugError::post("-", "SbVec3f c1(%g, %g, %g);", c1[0], c1[1], c1[2]);
329       SoDebugError::post("-", "SbVec3f a2(%g, %g, %g);", a2[0], a2[1], a2[2]);
330       SoDebugError::post("-", "SbVec3f b2(%g, %g, %g);", b2[0], b2[1], b2[2]);
331       SoDebugError::post("-", "SbVec3f c2(%g, %g, %g);", c2[0], c2[1], c2[2]);
332       assert(0);
333     }
334     return FALSE;
335   }
336 
337   SbVec3f p21, p22;
338   if (!plane1.intersect(SbLine(a2, b2), p21)) {
339     // should never happen
340     // but since it does, it means something
341     // possibly that a2 and b2 are in plane1, and halfspace values were wrong in
342     // some way.  we should either return FALSE or set p21 to something
343     if (SBTRI_DEBUG) {
344       SoDebugError::post("SbTri3f::intersect", "programming logic error 3");
345       SoDebugError::post("-", "SbVec3f a1(%g, %g, %g);", a1[0], a1[1], a1[2]);
346       SoDebugError::post("-", "SbVec3f b1(%g, %g, %g);", b1[0], b1[1], b1[2]);
347       SoDebugError::post("-", "SbVec3f c1(%g, %g, %g);", c1[0], c1[1], c1[2]);
348       SoDebugError::post("-", "SbVec3f a2(%g, %g, %g);", a2[0], a2[1], a2[2]);
349       SoDebugError::post("-", "SbVec3f b2(%g, %g, %g);", b2[0], b2[1], b2[2]);
350       SoDebugError::post("-", "SbVec3f c2(%g, %g, %g);", c2[0], c2[1], c2[2]);
351       assert(0);
352     }
353     return FALSE;
354   }
355   if (!plane1.intersect(SbLine(a2, c2), p22)) {
356     // should never happen
357     if (SBTRI_DEBUG) {
358       SoDebugError::post("SbTri3f::intersect", "programming logic error 4\n");
359       SoDebugError::post("-", "SbVec3f a1(%g, %g, %g);", a1[0], a1[1], a1[2]);
360       SoDebugError::post("-", "SbVec3f b1(%g, %g, %g);", b1[0], b1[1], b1[2]);
361       SoDebugError::post("-", "SbVec3f c1(%g, %g, %g);", c1[0], c1[1], c1[2]);
362       SoDebugError::post("-", "SbVec3f a2(%g, %g, %g);", a2[0], a2[1], a2[2]);
363       SoDebugError::post("-", "SbVec3f b2(%g, %g, %g);", b2[0], b2[1], b2[2]);
364       SoDebugError::post("-", "SbVec3f c2(%g, %g, %g);", c2[0], c2[1], c2[2]);
365       assert(0);
366     }
367     return FALSE;
368   }
369 
370   // find end point of the four (the one furtest from origo would be an end point)
371   // and the length of that line segment
372   float distance, maxdistance;
373   int vertex = 1;
374   maxdistance = p11.sqrLength();
375   distance = p12.sqrLength();
376   if (distance > maxdistance) {
377     vertex = 2;
378     maxdistance = distance;
379   }
380   distance = p21.sqrLength();
381   if (distance > maxdistance) {
382     vertex = 3;
383     maxdistance = distance;
384   }
385   distance = p22.sqrLength();
386   if (distance > maxdistance) {
387     vertex = 4;
388     maxdistance = distance;
389   }
390 
391   // check if a vertec from the other line segment is within the perimeter of the line
392   SbVec3f p, e, p1, p2;
393   switch (vertex) {
394   case 1:
395     p = p11; e = p12; p1 = p21; p2 = p22;
396     break;
397   case 2:
398     p = p12; e = p11; p1 = p21; p2 = p22;
399     break;
400   case 3:
401     p = p21; e = p22; p1 = p11; p2 = p12;
402     break;
403   case 4:
404     p = p22; e = p21; p1 = p11; p2 = p12;
405     break;
406   default:
407     if (SBTRI_DEBUG)
408       SoDebugError::post("SbTri3f::intersect", "programming logic error 5\n");
409     assert(0);
410   }
411   float pedistance = (e - p).sqrLength();
412   if (pedistance > (p1-p).sqrLength()) return TRUE;
413   if (pedistance > (p2-p).sqrLength()) return TRUE;
414   return FALSE;
415 }
416 
417 SbBool
intersect(const SbTri3f & t,float e) const418 SbTri3f::intersect(const SbTri3f & t, float e) const
419 {
420   if (e == 0.0f) return this->intersect(t);
421   if (this->getDistance(t) <= e) return TRUE;
422   return FALSE;
423 }
424 
425 SbVec3f
getNormal() const426 SbTri3f::getNormal() const
427 {
428   SbVec3f p[3];
429   this->getValue(p[0], p[1], p[2]);
430   SbPlane pl(p[0], p[1], p[2]);
431   return pl.getNormal();
432 }
433 
434 /*!
435   Returns the distance from the given point to this triangle.
436 */
437 float
getDistance(const SbVec3f & p) const438 SbTri3f::getDistance(const SbVec3f & p) const
439 {
440   float dist = FLT_MAX;
441   SbVec3f thisp[3];
442   this->getValue(thisp[0], thisp[1], thisp[2]);
443   SbPlane pl(thisp[0], thisp[1], thisp[2]);
444 
445   SbVec3f intersect;
446   SbVec3f n = this->getNormal();
447   SbLine line(p, p+n);
448   if (pl.intersect(line, intersect)) {
449     int i;
450     for (i=0;i<3;i++) {
451       SbPlane edgepl(thisp[i], thisp[i]+n, thisp[(i+1)%3]);
452       if (!edgepl.isInHalfSpace(intersect)) break;
453     }
454     if (i == 3) dist = static_cast<float>(fabs(pl.getDistance(p)));
455     else { // We didn't project inside triangle
456       for (int j=0;j<3;j++) {
457         float d = SbTri3f::getDistance(p, thisp[j], thisp[(j+1)%3]);
458         if (d < dist) dist = d;
459       }
460     }
461   }
462   else {
463     assert(FALSE);
464   }
465   return dist;
466 }
467 
468 /*!
469   Returns the distance from p to the line segment p1-p2.
470 */
471 float
getDistance(const SbVec3f & p,const SbVec3f & p1,const SbVec3f & p2)472 SbTri3f::getDistance(const SbVec3f & p,
473                    const SbVec3f & p1, const SbVec3f & p2)
474 {
475   SbVec3f normal = p2 - p1;
476   SbPlane pl1(normal, p1);
477   SbPlane pl2(-normal, p2);
478 
479   if (pl1.isInHalfSpace(p) && pl2.isInHalfSpace(p)) {
480     SbLine line(p1, p2);
481     return (line.getClosestPoint(p)-p).length();
482   }
483   else {
484     float d1 = (p - p1).length();
485     float d2 = (p - p2).length();
486     return (d1<d2)?d1:d2;
487   }
488 }
489 
490 static const float gs_fTolerance = 1e-06f;
491 
492 /*!
493   Returns the distance from this triangle to the given line segment.
494 */
495 float
getDistance(const SbVec3f & p1,const SbVec3f & p2) const496 SbTri3f::getDistance(const SbVec3f & p1, const SbVec3f & p2) const
497 {
498   SbVec3f kDiff = PRIVATE(this)->a - p1;
499   SbVec3f edge0 = PRIVATE(this)->b - PRIVATE(this)->a;
500   SbVec3f edge1 = PRIVATE(this)->c - PRIVATE(this)->a;
501   float fA00 = (p2-p1).sqrLength();
502   float fA01 = -(p2-p1).dot(edge0);
503   float fA02 = -(p2-p1).dot(edge1);
504   float fA11 = edge0.sqrLength();
505   float fA12 = edge0.dot(edge1);
506   float fA22 = edge1.dot(edge1);
507   float fB0  = -kDiff.dot(p2-p1);
508   float fB1  = kDiff.dot(edge0);
509   float fB2  = kDiff.dot(edge1);
510 
511   float fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;
512 
513   // Set up for a relative error test on the angle between ray direction
514   // and triangle normal to determine parallel/nonparallel status.
515   SbVec3f kN = edge0.cross(edge1);
516   float fNSqrLen = kN.sqrLength();
517   float fDot = (p2-p1).dot(kN);
518   SbBool bNotParallel = (fDot*fDot >= gs_fTolerance*fA00*fNSqrLen);
519 
520   if (bNotParallel) {
521     float fCof00 = fA11*fA22-fA12*fA12;
522     float fCof01 = fA02*fA12-fA01*fA22;
523     float fCof02 = fA01*fA12-fA02*fA11;
524     float fCof11 = fA00*fA22-fA02*fA02;
525     float fCof12 = fA02*fA01-fA00*fA12;
526     float fCof22 = fA00*fA11-fA01*fA01;
527     float fInvDet = 1.0f/(fA00*fCof00+fA01*fCof01+fA02*fCof02);
528     float fRhs0 = -fB0*fInvDet;
529     float fRhs1 = -fB1*fInvDet;
530     float fRhs2 = -fB2*fInvDet;
531 
532     fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
533     fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
534     fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;
535 
536     if (fR < 0.0f) {
537       if (fS+fT <= 1.0f) {
538         if (fS < 0.0f) {
539           if (fT < 0.0f) {  // region 4m
540             // min on face s=0 or t=0 or r=0
541             fSqrDist = SbTri3f::sqrDistance(p1, p2,
542                                           PRIVATE(this)->a, PRIVATE(this)->c,
543                                           &fR,&fT);
544             fS = 0.0f;
545             fSqrDist0 = SbTri3f::sqrDistance(p1, p2,
546                                            PRIVATE(this)->a, PRIVATE(this)->b,
547                                            &fR0,&fS0);
548             fT0 = 0.0f;
549             if (fSqrDist0 < fSqrDist) {
550               fSqrDist = fSqrDist0;
551               fR = fR0;
552               fS = fS0;
553               fT = fT0;
554             }
555             fSqrDist0 = this->sqrDistance(p1,&fS0,&fT0);
556             fR0 = 0.0f;
557             if (fSqrDist0 < fSqrDist) {
558               fSqrDist = fSqrDist0;
559               fR = fR0;
560               fS = fS0;
561               fT = fT0;
562             }
563           }
564           else {  // region 3m
565             // min on face s=0 or r=0
566             fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->c,&fR,&fT);
567             fS = 0.0f;
568             fSqrDist0 = this->sqrDistance(p1,&fS0,&fT0);
569             fR0 = 0.0f;
570             if (fSqrDist0 < fSqrDist) {
571               fSqrDist = fSqrDist0;
572               fR = fR0;
573               fS = fS0;
574               fT = fT0;
575             }
576           }
577         }
578         else if (fT < 0.0f) {  // region 5m
579           // min on face t=0 or r=0
580           fSqrDist = SbTri3f::sqrDistance(p1, p2,
581                                         PRIVATE(this)->a, PRIVATE(this)->b,
582                                         &fR,&fS);
583           fT = 0.0f;
584           fSqrDist0 = this->sqrDistance(p1,&fS0,&fT0);
585           fR0 = 0.0f;
586           if (fSqrDist0 < fSqrDist) {
587             fSqrDist = fSqrDist0;
588             fR = fR0;
589             fS = fS0;
590             fT = fT0;
591           }
592         }
593         else {  // region 0m
594           // min on face r=0
595           fSqrDist = this->sqrDistance(p1,&fS,&fT);
596           fR = 0.0f;
597         }
598       }
599       else {
600         if (fS < 0.0f) {  // region 2m
601           // min on face s=0 or s+t=1 or r=0
602           fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->c,&fR,&fT);
603           fS = 0.0f;
604           fSqrDist0 = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->b, PRIVATE(this)->c,&fR0,&fT0);
605           fS0 = 1.0f-fT0;
606           if (fSqrDist0 < fSqrDist) {
607             fSqrDist = fSqrDist0;
608             fR = fR0;
609             fS = fS0;
610             fT = fT0;
611           }
612           fSqrDist0 = this->sqrDistance(p1,&fS0,&fT0);
613           fR0 = 0.0f;
614           if (fSqrDist0 < fSqrDist) {
615             fSqrDist = fSqrDist0;
616             fR = fR0;
617             fS = fS0;
618             fT = fT0;
619           }
620         }
621         else if (fT < 0.0f) {  // region 6m
622           // min on face t=0 or s+t=1 or r=0
623           fSqrDist = SbTri3f::sqrDistance(p1, p2,
624                                         PRIVATE(this)->a, PRIVATE(this)->b,
625                                         &fR,&fS);
626           fT = 0.0f;
627           fSqrDist0 = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->b, PRIVATE(this)->c,&fR0,&fT0);
628           fS0 = 1.0f-fT0;
629           if (fSqrDist0 < fSqrDist) {
630             fSqrDist = fSqrDist0;
631             fR = fR0;
632             fS = fS0;
633             fT = fT0;
634           }
635           fSqrDist0 = this->sqrDistance(p1,&fS0,&fT0);
636           fR0 = 0.0f;
637           if (fSqrDist0 < fSqrDist) {
638             fSqrDist = fSqrDist0;
639             fR = fR0;
640             fS = fS0;
641             fT = fT0;
642           }
643         }
644         else {  // region 1m
645           // min on face s+t=1 or r=0
646           fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->b, PRIVATE(this)->c,&fR,&fT);
647           fS = 1.0f-fT;
648           fSqrDist0 = this->sqrDistance(p1,&fS0,&fT0);
649           fR0 = 0.0f;
650           if (fSqrDist0 < fSqrDist) {
651             fSqrDist = fSqrDist0;
652             fR = fR0;
653             fS = fS0;
654             fT = fT0;
655           }
656         }
657       }
658     }
659     else if (fR <= 1.0f) {
660       if (fS+fT <= 1.0f) {
661         if (fS < 0.0f) {
662           if (fT < 0.0f) {  // region 4
663             // min on face s=0 or t=0
664             fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->c,&fR,&fT);
665             fS = 0.0f;
666             fSqrDist0 = SbTri3f::sqrDistance(p1, p2,
667                                            PRIVATE(this)->a, PRIVATE(this)->b,
668                                            &fR0,&fS0);
669             fT0 = 0.0f;
670             if (fSqrDist0 < fSqrDist) {
671               fSqrDist = fSqrDist0;
672               fR = fR0;
673               fS = fS0;
674               fT = fT0;
675             }
676           }
677           else {  // region 3
678             // min on face s=0
679             fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->c,&fR,&fT);
680             fS = 0.0f;
681           }
682         }
683         else if (fT < 0.0f) {  // region 5
684           // min on face t=0
685           fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->b,&fR,&fS);
686           fT = 0.0f;
687         }
688         else {  // region 0
689           // global minimum is interior, done
690           fSqrDist = 0.0f;
691         }
692       }
693       else {
694         if (fS < 0.0f) {  // region 2
695           // min on face s=0 or s+t=1
696           fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->c,&fR,&fT);
697           fS = 0.0f;
698           fSqrDist0 = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->b, PRIVATE(this)->c,&fR0,&fT0);
699           fS0 = 1.0f-fT0;
700           if (fSqrDist0 < fSqrDist) {
701             fSqrDist = fSqrDist0;
702             fR = fR0;
703             fS = fS0;
704             fT = fT0;
705           }
706         }
707         else if (fT < 0.0f) {  // region 6
708           // min on face t=0 or s+t=1
709           fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->b,&fR,&fS);
710           fT = 0.0f;
711           fSqrDist0 = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->b, PRIVATE(this)->c,&fR0,&fT0);
712           fS0 = 1.0f-fT0;
713           if (fSqrDist0 < fSqrDist) {
714             fSqrDist = fSqrDist0;
715             fR = fR0;
716             fS = fS0;
717             fT = fT0;
718           }
719         }
720         else {  // region 1
721           // min on face s+t=1
722           fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->b, PRIVATE(this)->c,&fR,&fT);
723           fS = 1.0f-fT;
724         }
725       }
726     }
727     else {  // fR > 1
728       if (fS+fT <= 1.0f) {
729         if (fS < 0.0f) {
730           if (fT < 0.0f) {  // region 4p
731             // min on face s=0 or t=0 or r=1
732             fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->c,&fR,&fT);
733             fS = 0.0f;
734             fSqrDist0 = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->b,&fR0,&fS0);
735             fT0 = 0.0f;
736             if (fSqrDist0 < fSqrDist) {
737               fSqrDist = fSqrDist0;
738               fR = fR0;
739               fS = fS0;
740               fT = fT0;
741             }
742             fSqrDist0 = this->sqrDistance(p2,&fS0,&fT0);
743             fR0 = 1.0f;
744             if (fSqrDist0 < fSqrDist) {
745               fSqrDist = fSqrDist0;
746               fR = fR0;
747               fS = fS0;
748               fT = fT0;
749             }
750           }
751           else {  // region 3p
752             // min on face s=0 or r=1
753             fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->c,&fR,&fT);
754             fS = 0.0f;
755             fSqrDist0 = this->sqrDistance(p2,&fS0,&fT0);
756             fR0 = 1.0f;
757             if (fSqrDist0 < fSqrDist) {
758               fSqrDist = fSqrDist0;
759               fR = fR0;
760               fS = fS0;
761               fT = fT0;
762             }
763           }
764         }
765         else if (fT < 0.0f) {  // region 5p
766           // min on face t=0 or r=1
767           fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->b,&fR,&fS);
768           fT = 0.0f;
769           fSqrDist0 = this->sqrDistance(p2,&fS0,&fT0);
770           fR0 = 1.0f;
771           if (fSqrDist0 < fSqrDist) {
772             fSqrDist = fSqrDist0;
773             fR = fR0;
774             fS = fS0;
775             fT = fT0;
776           }
777         }
778         else {  // region 0p
779           // min face on r=1
780           fSqrDist = this->sqrDistance(p2,&fS,&fT);
781           fR = 1.0f;
782         }
783       }
784       else {
785         if (fS < 0.0f) {  // region 2p
786           // min on face s=0 or s+t=1 or r=1
787           fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->c,&fR,&fT);
788           fS = 0.0f;
789           fSqrDist0 = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->b, PRIVATE(this)->c,&fR0,&fT0);
790           fS0 = 1.0f-fT0;
791           if (fSqrDist0 < fSqrDist) {
792             fSqrDist = fSqrDist0;
793             fR = fR0;
794             fS = fS0;
795             fT = fT0;
796           }
797           fSqrDist0 = this->sqrDistance(p2,&fS0,&fT0);
798           fR0 = 1.0f;
799           if (fSqrDist0 < fSqrDist) {
800             fSqrDist = fSqrDist0;
801             fR = fR0;
802             fS = fS0;
803             fT = fT0;
804           }
805         }
806         else if (fT < 0.0f) {  // region 6p
807           // min on face t=0 or s+t=1 or r=1
808           fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->b,&fR,&fS);
809           fT = 0.0f;
810           fSqrDist0 = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->b, PRIVATE(this)->c,&fR0,&fT0);
811           fS0 = 1.0f-fT0;
812           if (fSqrDist0 < fSqrDist) {
813             fSqrDist = fSqrDist0;
814             fR = fR0;
815             fS = fS0;
816             fT = fT0;
817           }
818           fSqrDist0 = this->sqrDistance(p2,&fS0,&fT0);
819           fR0 = 1.0f;
820           if (fSqrDist0 < fSqrDist) {
821             fSqrDist = fSqrDist0;
822             fR = fR0;
823             fS = fS0;
824             fT = fT0;
825           }
826         }
827         else {  // region 1p
828           // min on face s+t=1 or r=1
829           fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->b, PRIVATE(this)->c,&fR,&fT);
830           fS = 1.0f-fT;
831           fSqrDist0 = this->sqrDistance(p2,&fS0,&fT0);
832           fR0 = 1.0f;
833           if (fSqrDist0 < fSqrDist) {
834             fSqrDist = fSqrDist0;
835             fR = fR0;
836             fS = fS0;
837             fT = fT0;
838           }
839         }
840       }
841     }
842   }
843   else {
844     // segment and triangle are parallel
845     fSqrDist = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->b,&fR,&fS);
846     fT = 0.0f;
847 
848     fSqrDist0 = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->a, PRIVATE(this)->c,&fR0,&fT0);
849     fS0 = 0.0f;
850     if (fSqrDist0 < fSqrDist) {
851       fSqrDist = fSqrDist0;
852       fR = fR0;
853       fS = fS0;
854       fT = fT0;
855     }
856 
857     fSqrDist0 = SbTri3f::sqrDistance(p1, p2, PRIVATE(this)->b, PRIVATE(this)->c,&fR0,&fT0);
858     fS0 = 1.0f-fT0;
859     if (fSqrDist0 < fSqrDist) {
860       fSqrDist = fSqrDist0;
861       fR = fR0;
862       fS = fS0;
863       fT = fT0;
864     }
865 
866     fSqrDist0 = this->sqrDistance(p1,&fS0,&fT0);
867     fR0 = 0.0f;
868     if (fSqrDist0 < fSqrDist)
869       {
870         fSqrDist = fSqrDist0;
871         fR = fR0;
872         fS = fS0;
873         fT = fT0;
874       }
875 
876     fSqrDist0 = this->sqrDistance(p2,&fS0,&fT0);
877     fR0 = 1.0f;
878     if (fSqrDist0 < fSqrDist)
879       {
880         fSqrDist = fSqrDist0;
881         fR = fR0;
882         fS = fS0;
883         fT = fT0;
884       }
885   }
886 
887 //    if (pfSegP) *pfSegP = fR;
888 //    if (pfTriP0) *pfTriP0 = fS;
889 //    if (pfTriP1) *pfTriP1 = fT;
890 
891   return static_cast<float>(sqrt(fSqrDist));
892 }
893 
894 /*!
895   Returns the distance between the two line segments.
896 */
897 float
sqrDistance(const SbVec3f & a1,const SbVec3f & a2,const SbVec3f & b1,const SbVec3f & b2,float * pfSegP0,float * pfSegP1)898 SbTri3f::sqrDistance(const SbVec3f & a1, const SbVec3f & a2,
899                    const SbVec3f & b1, const SbVec3f & b2,
900                    float * pfSegP0, float * pfSegP1)
901 {
902   SbVec3f kDiff = a1 - b1;
903   SbVec3f dir0 = a2 - a1;
904   SbVec3f dir1 = b2 - b1;
905   float fA00 = dir0.sqrLength();
906   float fA01 = -dir0.dot(dir1);
907   float fA11 = dir1.sqrLength();
908   float fB0 = kDiff.dot(dir0);
909   float fC = kDiff.sqrLength();
910   float fDet = static_cast<float>(fabs(fA00*fA11-fA01*fA01));
911   float fB1, fS, fT, fSqrDist, fTmp;
912 
913   if (fDet >= gs_fTolerance) {
914     // line segments are not parallel
915     fB1 = -kDiff.dot(dir1);
916     fS = fA01*fB1-fA11*fB0;
917     fT = fA01*fB0-fA00*fB1;
918 
919     if (fS >= 0.0f) {
920       if (fS <= fDet) {
921         if (fT >= 0.0f) {
922           if (fT <= fDet) {  // region 0 (interior)
923             // minimum at two interior points of 3D lines
924             float fInvDet = 1.0f/fDet;
925             fS *= fInvDet;
926             fT *= fInvDet;
927             fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
928               fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
929           }
930           else {  // region 3 (side)
931             fT = 1.0f;
932             fTmp = fA01+fB0;
933             if (fTmp >= 0.0f) {
934               fS = 0.0f;
935               fSqrDist = fA11+2.0f*fB1+fC;
936             }
937             else if (-fTmp >= fA00) {
938               fS = 1.0f;
939               fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
940             }
941             else {
942               fS = -fTmp/fA00;
943               fSqrDist = fTmp*fS+fA11+2.0f*fB1+fC;
944             }
945           }
946         }
947         else {  // region 7 (side)
948           fT = 0.0f;
949           if (fB0 >= 0.0f) {
950             fS = 0.0f;
951             fSqrDist = fC;
952           }
953           else if (-fB0 >= fA00) {
954             fS = 1.0f;
955             fSqrDist = fA00+2.0f*fB0+fC;
956           }
957           else {
958             fS = -fB0/fA00;
959             fSqrDist = fB0*fS+fC;
960           }
961         }
962       }
963       else {
964         if (fT >= 0.0) {
965           if (fT <= fDet) {  // region 1 (side)
966             fS = 1.0f;
967             fTmp = fA01+fB1;
968             if (fTmp >= 0.0f) {
969               fT = 0.0f;
970               fSqrDist = fA00+2.0f*fB0+fC;
971             }
972             else if (-fTmp >= fA11) {
973               fT = 1.0f;
974               fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
975             }
976             else {
977               fT = -fTmp/fA11;
978               fSqrDist = fTmp*fT+fA00+2.0f*fB0+fC;
979             }
980           }
981           else {  // region 2 (corner)
982             fTmp = fA01+fB0;
983             if (-fTmp <= fA00) {
984               fT = 1.0f;
985               if (fTmp >= 0.0f) {
986                 fS = 0.0f;
987                 fSqrDist = fA11+2.0f*fB1+fC;
988               }
989               else {
990                 fS = -fTmp/fA00;
991                 fSqrDist = fTmp*fS+fA11+2.0f*fB1+fC;
992               }
993             }
994             else {
995               fS = 1.0f;
996               fTmp = fA01+fB1;
997               if (fTmp >= 0.0f) {
998                 fT = 0.0f;
999                 fSqrDist = fA00+2.0f*fB0+fC;
1000               }
1001               else if (-fTmp >= fA11) {
1002                 fT = 1.0f;
1003                 fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
1004               }
1005               else {
1006                 fT = -fTmp/fA11;
1007                 fSqrDist = fTmp*fT+fA00+2.0f*fB0+fC;
1008               }
1009             }
1010           }
1011         }
1012         else {  // region 8 (corner)
1013           if (-fB0 < fA00) {
1014             fT = 0.0f;
1015             if (fB0 >= 0.0f) {
1016               fS = 0.0f;
1017               fSqrDist = fC;
1018             }
1019             else {
1020               fS = -fB0/fA00;
1021               fSqrDist = fB0*fS+fC;
1022             }
1023           }
1024           else {
1025             fS = 1.0f;
1026             fTmp = fA01+fB1;
1027             if (fTmp >= 0.0f) {
1028               fT = 0.0f;
1029               fSqrDist = fA00+2.0f*fB0+fC;
1030             }
1031             else if (-fTmp >= fA11) {
1032               fT = 1.0f;
1033               fSqrDist = fA00+fA11+fC+2.0f*(fB0+fTmp);
1034             }
1035             else {
1036               fT = -fTmp/fA11;
1037               fSqrDist = fTmp*fT+fA00+2.0f*fB0+fC;
1038             }
1039           }
1040         }
1041       }
1042     }
1043     else {
1044       if (fT >= 0.0f) {
1045         if (fT <= fDet) {  // region 5 (side)
1046           fS = 0.0f;
1047           if (fB1 >= 0.0f) {
1048             fT = 0.0f;
1049             fSqrDist = fC;
1050           }
1051           else if (-fB1 >= fA11) {
1052             fT = 1.0f;
1053             fSqrDist = fA11+2.0f*fB1+fC;
1054           }
1055           else {
1056             fT = -fB1/fA11;
1057             fSqrDist = fB1*fT+fC;
1058           }
1059         }
1060         else {  // region 4 (corner)
1061           fTmp = fA01+fB0;
1062           if (fTmp < 0.0f) {
1063             fT = 1.0f;
1064             if (-fTmp >= fA00) {
1065               fS = 1.0f;
1066               fSqrDist = fA00+fA11+fC+2.0f*(fB1+fTmp);
1067             }
1068             else {
1069               fS = -fTmp/fA00;
1070               fSqrDist = fTmp*fS+fA11+2.0f*fB1+fC;
1071             }
1072           }
1073           else {
1074             fS = 0.0f;
1075             if (fB1 >= 0.0f) {
1076               fT = 0.0f;
1077               fSqrDist = fC;
1078             }
1079             else if (-fB1 >= fA11) {
1080               fT = 1.0f;
1081               fSqrDist = fA11+2.0f*fB1+fC;
1082             }
1083             else {
1084               fT = -fB1/fA11;
1085               fSqrDist = fB1*fT+fC;
1086             }
1087           }
1088         }
1089       }
1090       else {   // region 6 (corner)
1091         if (fB0 < 0.0f) {
1092           fT = 0.0f;
1093           if (-fB0 >= fA00) {
1094             fS = 1.0f;
1095             fSqrDist = fA00+2.0f*fB0+fC;
1096           }
1097           else {
1098             fS = -fB0/fA00;
1099             fSqrDist = fB0*fS+fC;
1100           }
1101         }
1102         else {
1103           fS = 0.0f;
1104           if (fB1 >= 0.0f) {
1105             fT = 0.0f;
1106             fSqrDist = fC;
1107           }
1108           else if (-fB1 >= fA11) {
1109             fT = 1.0f;
1110             fSqrDist = fA11+2.0f*fB1+fC;
1111           }
1112           else {
1113             fT = -fB1/fA11;
1114             fSqrDist = fB1*fT+fC;
1115           }
1116         }
1117       }
1118     }
1119   }
1120   else {
1121     // line segments are parallel
1122     if (fA01 > 0.0f) {
1123       // direction vectors form an obtuse angle
1124       if (fB0 >= 0.0f) {
1125         fS = 0.0f;
1126         fT = 0.0f;
1127         fSqrDist = fC;
1128       }
1129       else if (-fB0 <= fA00) {
1130         fS = -fB0/fA00;
1131         fT = 0.0f;
1132         fSqrDist = fB0*fS+fC;
1133       }
1134       else {
1135         fB1 = -kDiff.dot(dir1);
1136         fS = 1.0f;
1137         fTmp = fA00+fB0;
1138         if (-fTmp >= fA01) {
1139           fT = 1.0f;
1140           fSqrDist = fA00+fA11+fC+2.0f*(fA01+fB0+fB1);
1141         }
1142         else {
1143           fT = -fTmp/fA01;
1144           fSqrDist = fA00+2.0f*fB0+fC+fT*(fA11*fT+2.0f*(fA01+fB1));
1145         }
1146       }
1147     }
1148     else {
1149       // direction vectors form an acute angle
1150       if (-fB0 >= fA00) {
1151         fS = 1.0f;
1152         fT = 0.0f;
1153         fSqrDist = fA00+2.0f*fB0+fC;
1154       }
1155       else if (fB0 <= 0.0f) {
1156         fS = -fB0/fA00;
1157         fT = 0.0f;
1158         fSqrDist = fB0*fS+fC;
1159       }
1160       else {
1161         fB1 = -kDiff.dot(dir1);
1162         fS = 0.0f;
1163         if (fB0 >= -fA01) {
1164           fT = 1.0f;
1165           fSqrDist = fA11+2.0f*fB1+fC;
1166         }
1167         else {
1168           fT = -fB0/fA01;
1169           fSqrDist = fC+fT*(2.0f*fB1+fA11*fT);
1170         }
1171       }
1172     }
1173   }
1174 
1175   if (pfSegP0) *pfSegP0 = fS;
1176 
1177   if (pfSegP1) *pfSegP1 = fT;
1178 
1179   return static_cast<float>(fabs(fSqrDist));
1180 
1181   //FIXME: Old code. not for segments but lines.
1182 #if 0
1183   SbVec3f kDiff = a1 - b1;
1184   SbVec3f dir0 = a2 - a1;
1185   SbVec3f dir1 = b2 - b1;
1186   float fA00 = dir0.sqrLength();
1187   float fA01 = -dir0.dot(dir1);
1188   float fA11 = dir1.sqrLength();
1189   float fB0 = kDiff.dot(dir0);
1190   float fC = kDiff.sqrLength();
1191   float fDet = fabs(fA00*fA11-fA01*fA01);
1192   float fB1, fS, fT, fSqrDist;
1193 
1194   if (fDet >= gs_fTolerance) {
1195     // lines are not parallel
1196     fB1 = -kDiff.dot(dir1);
1197     float fInvDet = 1.0f/fDet;
1198     fS = (fA01*fB1-fA11*fB0)*fInvDet;
1199     fT = (fA01*fB0-fA00*fB1)*fInvDet;
1200     fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
1201       fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
1202   }
1203   else {
1204     // lines are parallel, select any closest pair of points
1205     fS = -fB0/fA00;
1206     fT = 0.0f;
1207     fSqrDist = fB0*fS+fC;
1208   }
1209 
1210   if (linP0) *linP0 = fS;
1211 
1212   if (linP1) *linP1 = fT;
1213 
1214   return fabs(fSqrDist);
1215 #endif
1216 }
1217 
1218 /*!
1219   Returns the minimum distance from this triangle to the given triangle.
1220 */
1221 float
getDistance(const SbTri3f & t) const1222 SbTri3f::getDistance(const SbTri3f & t) const
1223 {
1224   float dist = FLT_MAX;
1225   SbVec3f p[3];
1226   t.getValue(p[0], p[1], p[2]);
1227   int i;
1228   for (i=0;i<3;i++) {
1229     float d = this->getDistance(p[i], p[(i+1)%3]);
1230     if (d < dist) dist = d;
1231   }
1232   this->getValue(p[0], p[1], p[2]);
1233   for (i=0;i<3;i++) {
1234     float d = t.getDistance(p[i], p[(i+1)%3]);
1235     if (d < dist) dist = d;
1236   }
1237 
1238 #if 0 // debug output
1239   if (dist <= 1) {
1240     this->getValue(p[0], p[1], p[2]);
1241     for (int i=0;i<3;i++) {
1242       printf("%f %f %f (%x %x %x)\n",
1243              p[i][0], p[i][1], p[i][2],
1244              *(void **)(&p[i][0]), *(void **)(&p[i][1]), *(void **)(&p[i][2]));
1245     }
1246     t.getValue(p[0], p[1], p[2]);
1247     for (int i=0;i<3;i++) {
1248       printf("%f %f %f (%x %x %x)\n",
1249              p[i][0], p[i][1], p[i][2],
1250              *(void **)(&p[i][0]), *(void **)(&p[i][1]), *(void **)(&p[i][2]));
1251     }
1252     printf("Dist: %f\n", dist);
1253   }
1254 #endif
1255 
1256   return dist;
1257 
1258   //FIXME: Old code. Kept for reference (kintel 20021121)
1259 #if 0
1260   float dist = FLT_MAX;
1261   SbVec3f p[3];
1262   t.getValue(p[0], p[1], p[2]);
1263   int i;
1264   for (i=0;i<3;i++) {
1265     float d = this->getDistance(p[i]);
1266     if (d < dist) dist = d;
1267   }
1268   this->getValue(p[0], p[1], p[2]);
1269   for (i=0;i<3;i++) {
1270     float d = t.getDistance(p[i]);
1271     if (d < dist) dist = d;
1272   }
1273 
1274   return dist;
1275 #endif
1276 }
1277 
1278 float
sqrDistance(const SbVec3f & p1,float * pfSParam,float * pfTParam) const1279 SbTri3f::sqrDistance (const SbVec3f & p1,
1280                     float * pfSParam, float * pfTParam) const
1281 {
1282   SbVec3f kDiff = PRIVATE(this)->a - p1;
1283   SbVec3f edge0 = PRIVATE(this)->b - PRIVATE(this)->a;
1284   SbVec3f edge1 = PRIVATE(this)->c - PRIVATE(this)->a;
1285   float fA00 = edge0.sqrLength();
1286   float fA01 = edge0.dot(edge1);
1287   float fA11 = edge1.sqrLength();
1288   float fB0 = kDiff.dot(edge0);
1289   float fB1 = kDiff.dot(edge1);
1290   float fC = kDiff.sqrLength();
1291   float fDet = static_cast<float>(fabs(fA00*fA11-fA01*fA01));
1292   float fS = fA01*fB1-fA11*fB0;
1293   float fT = fA01*fB0-fA00*fB1;
1294   float fSqrDist;
1295 
1296   if (fS + fT <= fDet)
1297     {
1298       if (fS < 0.0f)
1299         {
1300           if (fT < 0.0f)  // region 4
1301             {
1302               if (fB0 < 0.0f)
1303                 {
1304                   fT = 0.0f;
1305                   if (-fB0 >= fA00)
1306                     {
1307                       fS = 1.0f;
1308                       fSqrDist = fA00+2.0f*fB0+fC;
1309                     }
1310                   else
1311                     {
1312                       fS = -fB0/fA00;
1313                       fSqrDist = fB0*fS+fC;
1314                     }
1315                 }
1316               else
1317                 {
1318                   fS = 0.0f;
1319                   if (fB1 >= 0.0f)
1320                     {
1321                       fT = 0.0f;
1322                       fSqrDist = fC;
1323                     }
1324                   else if (-fB1 >= fA11)
1325                     {
1326                       fT = 1.0f;
1327                       fSqrDist = fA11+2.0f*fB1+fC;
1328                     }
1329                   else
1330                     {
1331                       fT = -fB1/fA11;
1332                       fSqrDist = fB1*fT+fC;
1333                     }
1334                 }
1335             }
1336           else  // region 3
1337             {
1338               fS = 0.0f;
1339               if (fB1 >= 0.0f)
1340                 {
1341                   fT = 0.0f;
1342                   fSqrDist = fC;
1343                 }
1344               else if (-fB1 >= fA11)
1345                 {
1346                   fT = 1.0f;
1347                   fSqrDist = fA11+2.0f*fB1+fC;
1348                 }
1349               else
1350                 {
1351                   fT = -fB1/fA11;
1352                   fSqrDist = fB1*fT+fC;
1353                 }
1354             }
1355         }
1356       else if (fT < 0.0f)  // region 5
1357         {
1358           fT = 0.0f;
1359           if (fB0 >= 0.0f)
1360             {
1361               fS = 0.0f;
1362               fSqrDist = fC;
1363             }
1364           else if (-fB0 >= fA00)
1365             {
1366               fS = 1.0f;
1367               fSqrDist = fA00+2.0f*fB0+fC;
1368             }
1369           else
1370             {
1371               fS = -fB0/fA00;
1372               fSqrDist = fB0*fS+fC;
1373             }
1374         }
1375       else  // region 0
1376         {
1377           // minimum at interior point
1378           float fInvDet = 1.0f/fDet;
1379           fS *= fInvDet;
1380           fT *= fInvDet;
1381           fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
1382             fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
1383         }
1384     }
1385   else
1386     {
1387       float fTmp0, fTmp1, fNumer, fDenom;
1388 
1389       if (fS < 0.0f)  // region 2
1390         {
1391           fTmp0 = fA01 + fB0;
1392           fTmp1 = fA11 + fB1;
1393           if (fTmp1 > fTmp0)
1394             {
1395               fNumer = fTmp1 - fTmp0;
1396               fDenom = fA00-2.0f*fA01+fA11;
1397               if (fNumer >= fDenom)
1398                 {
1399                   fS = 1.0f;
1400                   fT = 0.0f;
1401                   fSqrDist = fA00+2.0f*fB0+fC;
1402                 }
1403               else
1404                 {
1405                   fS = fNumer/fDenom;
1406                   fT = 1.0f - fS;
1407                   fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
1408                     fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
1409                 }
1410             }
1411           else
1412             {
1413               fS = 0.0f;
1414               if (fTmp1 <= 0.0f)
1415                 {
1416                   fT = 1.0f;
1417                   fSqrDist = fA11+2.0f*fB1+fC;
1418                 }
1419               else if (fB1 >= 0.0f)
1420                 {
1421                   fT = 0.0f;
1422                   fSqrDist = fC;
1423                 }
1424               else
1425                 {
1426                   fT = -fB1/fA11;
1427                   fSqrDist = fB1*fT+fC;
1428                 }
1429             }
1430         }
1431       else if (fT < 0.0f)  // region 6
1432         {
1433           fTmp0 = fA01 + fB1;
1434           fTmp1 = fA00 + fB0;
1435           if (fTmp1 > fTmp0)
1436             {
1437               fNumer = fTmp1 - fTmp0;
1438               fDenom = fA00-2.0f*fA01+fA11;
1439               if (fNumer >= fDenom)
1440                 {
1441                   fT = 1.0f;
1442                   fS = 0.0f;
1443                   fSqrDist = fA11+2.0f*fB1+fC;
1444                 }
1445               else
1446                 {
1447                   fT = fNumer/fDenom;
1448                   fS = 1.0f - fT;
1449                   fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
1450                     fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
1451                 }
1452             }
1453           else
1454             {
1455               fT = 0.0f;
1456               if (fTmp1 <= 0.0f)
1457                 {
1458                   fS = 1.0f;
1459                   fSqrDist = fA00+2.0f*fB0+fC;
1460                 }
1461               else if (fB0 >= 0.0f)
1462                 {
1463                   fS = 0.0f;
1464                   fSqrDist = fC;
1465                 }
1466               else
1467                 {
1468                   fS = -fB0/fA00;
1469                   fSqrDist = fB0*fS+fC;
1470                 }
1471             }
1472         }
1473       else  // region 1
1474         {
1475           fNumer = fA11 + fB1 - fA01 - fB0;
1476           if (fNumer <= 0.0f)
1477             {
1478               fS = 0.0f;
1479               fT = 1.0f;
1480               fSqrDist = fA11+2.0f*fB1+fC;
1481             }
1482           else
1483             {
1484               fDenom = fA00-2.0f*fA01+fA11;
1485               if (fNumer >= fDenom)
1486                 {
1487                   fS = 1.0f;
1488                   fT = 0.0f;
1489                   fSqrDist = fA00+2.0f*fB0+fC;
1490                 }
1491               else
1492                 {
1493                   fS = fNumer/fDenom;
1494                   fT = 1.0f - fS;
1495                   fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
1496                     fT*(fA01*fS+fA11*fT+2.0f*fB1)+fC;
1497                 }
1498             }
1499         }
1500     }
1501 
1502   if (pfSParam)
1503     *pfSParam = fS;
1504 
1505   if (pfTParam)
1506     *pfTParam = fT;
1507 
1508   return static_cast<float>(fabs(fSqrDist));
1509 }
1510 
1511 // *************************************************************************
1512 
1513 /*!
1514   Returns bounding box fully enclosing the triangle.
1515 */
1516 const SbBox3f
getBoundingBox(void) const1517 SbTri3f::getBoundingBox(void) const
1518 {
1519   // FIXME: this involves quite a lot of function calls, and can
1520   // probably be optimized simply by expanding the code. 20030328 mortene.
1521 
1522   SbBox3f b;
1523   b.extendBy(PRIVATE(this)->a);
1524   b.extendBy(PRIVATE(this)->b);
1525   b.extendBy(PRIVATE(this)->c);
1526   return b;
1527 }
1528 
1529 // *************************************************************************
1530 
1531 #undef SBTRI_DEBUG
1532 #undef PRIVATE
1533