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