simgear/math/SGIntersect.hxx

   1 // Copyright (C) 2006-2009  Mathias Froehlich - Mathias.Froehlich@web.de
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Library General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Library General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU General Public License
  14 // along with this program; if not, write to the Free Software
  15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  16 //
  17
  18 #ifndef SGIntersect_HXX
  19 #define SGIntersect_HXX
  20
  21 template<typename T>
  22 inline bool
  23 intersects(const SGSphere<T>& s1, const SGSphere<T>& s2)
  24 {
  25   if (s1.empty())
  26     return false;
  27   if (s2.empty())
  28     return false;
  29
  30   T dist = s1.getRadius() + s2.getRadius();
  31   return distSqr(s1.getCenter(), s2.getCenter()) <= dist*dist;
  32 }
  33
  34
  35 template<typename T1, typename T2>
  36 inline bool
  37 intersects(const SGBox<T1>& box, const SGSphere<T2>& sphere)
  38 {
  39   if (sphere.empty())
  40     return false;
  41   if (box.empty())
  42     return false;
  43
  44   SGVec3<T1> closest = box.getClosestPoint(sphere.getCenter());
  45   return distSqr(closest, SGVec3<T1>(sphere.getCenter())) <= sphere.getRadius2();
  46 }
  47 // make it symmetric
  48 template<typename T1, typename T2>
  49 inline bool
  50 intersects(const SGSphere<T1>& sphere, const SGBox<T2>& box)
  51 { return intersects(box, sphere); }
  52
  53
  54 template<typename T1, typename T2>
  55 inline bool
  56 intersects(const SGVec3<T1>& v, const SGBox<T2>& box)
  57 {
  58   if (v[0] < box.getMin()[0])
  59     return false;
  60   if (box.getMax()[0] < v[0])
  61     return false;
  62   if (v[1] < box.getMin()[1])
  63     return false;
  64   if (box.getMax()[1] < v[1])
  65     return false;
  66   if (v[2] < box.getMin()[2])
  67     return false;
  68   if (box.getMax()[2] < v[2])
  69     return false;
  70   return true;
  71 }
  72 template<typename T1, typename T2>
  73 inline bool
  74 intersects(const SGBox<T1>& box, const SGVec3<T2>& v)
  75 { return intersects(v, box); }
  76
  77
  78 template<typename T>
  79 inline bool
  80 intersects(const SGRay<T>& ray, const SGPlane<T>& plane)
  81 {
  82   // We compute the intersection point
  83   //   x = origin + \alpha*direction
  84   // from the ray origin and non nomalized direction.
  85   // For 0 <= \alpha the ray intersects the infinite plane.
  86   // The intersection point x can also be written
  87   //   x = n*dist + y
  88   // where n is the planes normal, dist is the distance of the plane from
  89   // the origin in normal direction and y is ana aproriate vector
  90   // perpendicular to n.
  91   // Equate the x values and take the scalar product with the plane normal n.
  92   //   dot(n, origin) + \alpha*dot(n, direction) = dist
  93   // We can now compute alpha from the above equation.
  94   //   \alpha = (dist - dot(n, origin))/dot(n, direction)
  95
  96   // The negative numerator for the \alpha expression
  97   T num = plane.getPositiveDist();
  98   num -= dot(plane.getNormal(), ray.getOrigin());
  99
 100   // If the numerator is zero, we have the rays origin included in the plane
 101   if (fabs(num) <= SGLimits<T>::min())
 102     return true;
 103
 104   // The denominator for the \alpha expression
 105   T den = dot(plane.getNormal(), ray.getDirection());
 106
 107   // If we get here, we already know that the rays origin is not included
 108   // in the plane. Thus if we have a zero denominator we have
 109   // a ray paralell to the plane. That is no intersection.
 110   if (fabs(den) <= SGLimits<T>::min())
 111     return false;
 112
 113   // We would now compute \alpha = num/den and compare with 0 and 1.
 114   // But to avoid that expensive division, check equation multiplied by
 115   // the denominator.
 116   T alphaDen = copysign(1, den)*num;
 117   if (alphaDen < 0)
 118     return false;
 119
 120   return true;
 121 }
 122 // make it symmetric
 123 template<typename T>
 124 inline bool
 125 intersects(const SGPlane<T>& plane, const SGRay<T>& ray)
 126 { return intersects(ray, plane); }
 127
 128 template<typename T>
 129 inline bool
 130 intersects(SGVec3<T>& dst, const SGRay<T>& ray, const SGPlane<T>& plane)
 131 {
 132   // We compute the intersection point
 133   //   x = origin + \alpha*direction
 134   // from the ray origin and non nomalized direction.
 135   // For 0 <= \alpha the ray intersects the infinite plane.
 136   // The intersection point x can also be written
 137   //   x = n*dist + y
 138   // where n is the planes normal, dist is the distance of the plane from
 139   // the origin in normal direction and y is ana aproriate vector
 140   // perpendicular to n.
 141   // Equate the x values and take the scalar product with the plane normal n.
 142   //   dot(n, origin) + \alpha*dot(n, direction) = dist
 143   // We can now compute alpha from the above equation.
 144   //   \alpha = (dist - dot(n, origin))/dot(n, direction)
 145
 146   // The negative numerator for the \alpha expression
 147   T num = plane.getPositiveDist();
 148   num -= dot(plane.getNormal(), ray.getOrigin());
 149
 150   // If the numerator is zero, we have the rays origin included in the plane
 151   if (fabs(num) <= SGLimits<T>::min()) {
 152     dst = ray.getOrigin();
 153     return true;
 154   }
 155
 156   // The denominator for the \alpha expression
 157   T den = dot(plane.getNormal(), ray.getDirection());
 158
 159   // If we get here, we already know that the rays origin is not included
 160   // in the plane. Thus if we have a zero denominator we have
 161   // a ray paralell to the plane. That is no intersection.
 162   if (fabs(den) <= SGLimits<T>::min())
 163     return false;
 164
 165   // We would now compute \alpha = num/den and compare with 0 and 1.
 166   // But to avoid that expensive division, check equation multiplied by
 167   // the denominator.
 168   T alpha = num/den;
 169   if (alpha < 0)
 170     return false;
 171
 172   dst = ray.getOrigin() + alpha*ray.getDirection();
 173   return true;
 174 }
 175 // make it symmetric
 176 template<typename T>
 177 inline bool
 178 intersects(SGVec3<T>& dst, const SGPlane<T>& plane, const SGRay<T>& ray)
 179 { return intersects(dst, ray, plane); }
 180
 181 template<typename T>
 182 inline bool
 183 intersects(const SGLineSegment<T>& lineSegment, const SGPlane<T>& plane)
 184 {
 185   // We compute the intersection point
 186   //   x = origin + \alpha*direction
 187   // from the line segments origin and non nomalized direction.
 188   // For 0 <= \alpha <= 1 the line segment intersects the infinite plane.
 189   // The intersection point x can also be written
 190   //   x = n*dist + y
 191   // where n is the planes normal, dist is the distance of the plane from
 192   // the origin in normal direction and y is ana aproriate vector
 193   // perpendicular to n.
 194   // Equate the x values and take the scalar product with the plane normal n.
 195   //   dot(n, origin) + \alpha*dot(n, direction) = dist
 196   // We can now compute alpha from the above equation.
 197   //   \alpha = (dist - dot(n, origin))/dot(n, direction)
 198
 199   // The negative numerator for the \alpha expression
 200   T num = plane.getPositiveDist();
 201   num -= dot(plane.getNormal(), lineSegment.getOrigin());
 202
 203   // If the numerator is zero, we have the lines origin included in the plane
 204   if (fabs(num) <= SGLimits<T>::min())
 205     return true;
 206
 207   // The denominator for the \alpha expression
 208   T den = dot(plane.getNormal(), lineSegment.getDirection());
 209
 210   // If we get here, we already know that the lines origin is not included
 211   // in the plane. Thus if we have a zero denominator we have
 212   // a line paralell to the plane. That is no intersection.
 213   if (fabs(den) <= SGLimits<T>::min())
 214     return false;
 215
 216   // We would now compute \alpha = num/den and compare with 0 and 1.
 217   // But to avoid that expensive division, compare equations
 218   // multiplied by |den|. Note that copysign is usually a compiler intrinsic
 219   // that expands in assembler code that not even stalls the cpus pipes.
 220   T alphaDen = copysign(1, den)*num;
 221   if (alphaDen < 0)
 222     return false;
 223   if (den < alphaDen)
 224     return false;
 225
 226   return true;
 227 }
 228 // make it symmetric
 229 template<typename T>
 230 inline bool
 231 intersects(const SGPlane<T>& plane, const SGLineSegment<T>& lineSegment)
 232 { return intersects(lineSegment, plane); }
 233
 234 template<typename T>
 235 inline bool
 236 intersects(SGVec3<T>& dst, const SGLineSegment<T>& lineSegment, const SGPlane<T>& plane)
 237 {
 238   // We compute the intersection point
 239   //   x = origin + \alpha*direction
 240   // from the line segments origin and non nomalized direction.
 241   // For 0 <= \alpha <= 1 the line segment intersects the infinite plane.
 242   // The intersection point x can also be written
 243   //   x = n*dist + y
 244   // where n is the planes normal, dist is the distance of the plane from
 245   // the origin in normal direction and y is an aproriate vector
 246   // perpendicular to n.
 247   // Equate the x values and take the scalar product with the plane normal n.
 248   //   dot(n, origin) + \alpha*dot(n, direction) = dist
 249   // We can now compute alpha from the above equation.
 250   //   \alpha = (dist - dot(n, origin))/dot(n, direction)
 251
 252   // The negative numerator for the \alpha expression
 253   T num = plane.getPositiveDist();
 254   num -= dot(plane.getNormal(), lineSegment.getOrigin());
 255
 256   // If the numerator is zero, we have the lines origin included in the plane
 257   if (fabs(num) <= SGLimits<T>::min()) {
 258     dst = lineSegment.getOrigin();
 259     return true;
 260   }
 261
 262   // The denominator for the \alpha expression
 263   T den = dot(plane.getNormal(), lineSegment.getDirection());
 264
 265   // If we get here, we already know that the lines origin is not included
 266   // in the plane. Thus if we have a zero denominator we have
 267   // a line paralell to the plane. That is: no intersection.
 268   if (fabs(den) <= SGLimits<T>::min())
 269     return false;
 270
 271   // We would now compute \alpha = num/den and compare with 0 and 1.
 272   // But to avoid that expensive division, check equation multiplied by
 273   // the denominator. FIXME: shall we do so? or compute like that?
 274   T alpha = num/den;
 275   if (alpha < 0)
 276     return false;
 277   if (1 < alpha)
 278     return false;
 279
 280   dst = lineSegment.getOrigin() + alpha*lineSegment.getDirection();
 281   return true;
 282 }
 283 // make it symmetric
 284 template<typename T>
 285 inline bool
 286 intersects(SGVec3<T>& dst, const SGPlane<T>& plane, const SGLineSegment<T>& lineSegment)
 287 { return intersects(dst, lineSegment, plane); }
 288
 289
 290 // Distance of a line segment to a point
 291 template<typename T>
 292 inline T
 293 distSqr(const SGLineSegment<T>& lineSeg, const SGVec3<T>& p)
 294 {
 295   SGVec3<T> ps = p - lineSeg.getStart();
 296
 297   T psdotdir = dot(ps, lineSeg.getDirection());
 298   if (psdotdir <= 0)
 299     return dot(ps, ps);
 300
 301   SGVec3<T> pe = p - lineSeg.getEnd();
 302   if (0 <= dot(pe, lineSeg.getDirection()))
 303     return dot(pe, pe);
 304
 305   return dot(ps, ps) - psdotdir*psdotdir/dot(lineSeg.getDirection(), lineSeg.getDirection());
 306 }
 307 // make it symmetric
 308 template<typename T>
 309 inline T
 310 distSqr(const SGVec3<T>& p, const SGLineSegment<T>& lineSeg)
 311 { return distSqr(lineSeg, p); }
 312 // with sqrt
 313 template<typename T>
 314 inline T
 315 dist(const SGVec3<T>& p, const SGLineSegment<T>& lineSeg)
 316 { return sqrt(distSqr(lineSeg, p)); }
 317 template<typename T>
 318 inline T
 319 dist(const SGLineSegment<T>& lineSeg, const SGVec3<T>& p)
 320 { return sqrt(distSqr(lineSeg, p)); }
 321
 322 template<typename T>
 323 inline bool
 324 intersects(const SGRay<T>& ray, const SGSphere<T>& sphere)
 325 {
 326   // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering,
 327   // second edition, page 571
 328   SGVec3<T> l = sphere.getCenter() - ray.getOrigin();
 329   T s = dot(l, ray.getDirection());
 330   T l2 = dot(l, l);
 331
 332   T r2 = sphere.getRadius2();
 333   if (s < 0 && l2 > r2)
 334     return false;
 335
 336   T d2 = dot(ray.getDirection(), ray.getDirection());
 337   // The original test would read
 338   //   T m2 = l2 - s*s/d2;
 339   //   if (m2 > r2)
 340   //     return false;
 341   // but to avoid the expensive division, we multiply by d2
 342   T m2 = d2*l2 - s*s;
 343   if (m2 > d2*r2)
 344     return false;
 345
 346   return true;
 347 }
 348 // make it symmetric
 349 template<typename T>
 350 inline bool
 351 intersects(const SGSphere<T>& sphere, const SGRay<T>& ray)
 352 { return intersects(ray, sphere); }
 353
 354 template<typename T>
 355 inline bool
 356 intersects(const SGLineSegment<T>& lineSegment, const SGSphere<T>& sphere)
 357 {
 358   // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering,
 359   // second edition, page 571
 360   SGVec3<T> l = sphere.getCenter() - lineSegment.getStart();
 361   T ld = length(lineSegment.getDirection());
 362   T s = dot(l, lineSegment.getDirection())/ld;
 363   T l2 = dot(l, l);
 364
 365   T r2 = sphere.getRadius2();
 366   if (s < 0 && l2 > r2)
 367     return false;
 368
 369   T m2 = l2 - s*s;
 370   if (m2 > r2)
 371     return false;
 372
 373   T q = sqrt(r2 - m2);
 374   T t = s - q;
 375   if (ld < t)
 376     return false;
 377
 378   return true;
 379 }
 380 // make it symmetric
 381 template<typename T>
 382 inline bool
 383 intersects(const SGSphere<T>& sphere, const SGLineSegment<T>& lineSegment)
 384 { return intersects(lineSegment, sphere); }
 385
 386
 387 template<typename T>
 388 inline bool
 389 // FIXME do not use that default argument later. Just for development now
 390 intersects(SGVec3<T>& x, const SGTriangle<T>& tri, const SGRay<T>& ray, T eps = 0)
 391 {
 392   // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
 393
 394   // Method based on the observation that we are looking for a
 395   // point x that can be expressed in terms of the triangle points
 396   //  x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
 397   // with 0 <= u, v and u + v <= 1.
 398   // OTOH it could be expressed in terms of the ray
 399   //  x = o + t*d
 400   // Now we can compute u, v and t.
 401   SGVec3<T> p = cross(ray.getDirection(), tri.getEdge(1));
 402
 403   T denom = dot(p, tri.getEdge(0));
 404   T signDenom = copysign(1, denom);
 405
 406   SGVec3<T> s = ray.getOrigin() - tri.getBaseVertex();
 407   SGVec3<T> q = cross(s, tri.getEdge(0));
 408   // Now t would read
 409   //   t = 1/denom*dot(q, tri.getEdge(1));
 410   // To avoid an expensive division we multiply by |denom|
 411   T tDenom = signDenom*dot(q, tri.getEdge(1));
 412   if (tDenom < 0)
 413     return false;
 414   // For line segment we would test against
 415   // if (1 < t)
 416   //   return false;
 417   // with the original t. The multiplied test would read
 418   // if (absDenom < tDenom)
 419   //   return false;
 420
 421   T absDenom = fabs(denom);
 422   T absDenomEps = absDenom*eps;
 423
 424   // T u = 1/denom*dot(p, s);
 425   T u = signDenom*dot(p, s);
 426   if (u < -absDenomEps)
 427     return false;
 428   // T v = 1/denom*dot(q, d);
 429   // if (v < -eps)
 430   //   return false;
 431   T v = signDenom*dot(q, ray.getDirection());
 432   if (v < -absDenomEps)
 433     return false;
 434
 435   if (u + v > absDenom + absDenomEps)
 436     return false;
 437
 438   // return if paralell ??? FIXME what if paralell and in plane?
 439   // may be we are ok below than anyway??
 440   if (absDenom <= SGLimits<T>::min())
 441     return false;
 442
 443   x = ray.getOrigin();
 444   // if we have survived here it could only happen with denom == 0
 445   // that the point is already in plane. Then return the origin ...
 446   if (SGLimitsd::min() < absDenom)
 447     x += (tDenom/absDenom)*ray.getDirection();
 448
 449   return true;
 450 }
 451
 452 template<typename T>
 453 inline bool
 454 intersects(const SGTriangle<T>& tri, const SGRay<T>& ray, T eps = 0)
 455 {
 456   // FIXME: for now just wrap the other method. When that has prooven
 457   // well optimized, implement that special case
 458   SGVec3<T> dummy;
 459   return intersects(dummy, tri, ray, eps);
 460 }
 461
 462 template<typename T>
 463 inline bool
 464 // FIXME do not use that default argument later. Just for development now
 465 intersects(SGVec3<T>& x, const SGTriangle<T>& tri, const SGLineSegment<T>& lineSegment, T eps = 0)
 466 {
 467   // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
 468
 469   // Method based on the observation that we are looking for a
 470   // point x that can be expressed in terms of the triangle points
 471   //  x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
 472   // with 0 <= u, v and u + v <= 1.
 473   // OTOH it could be expressed in terms of the lineSegment
 474   //  x = o + t*d
 475   // Now we can compute u, v and t.
 476   SGVec3<T> p = cross(lineSegment.getDirection(), tri.getEdge(1));
 477
 478   T denom = dot(p, tri.getEdge(0));
 479   T signDenom = copysign(1, denom);
 480
 481   SGVec3<T> s = lineSegment.getStart() - tri.getBaseVertex();
 482   SGVec3<T> q = cross(s, tri.getEdge(0));
 483   // Now t would read
 484   //   t = 1/denom*dot(q, tri.getEdge(1));
 485   // To avoid an expensive division we multiply by |denom|
 486   T tDenom = signDenom*dot(q, tri.getEdge(1));
 487   if (tDenom < 0)
 488     return false;
 489   // For line segment we would test against
 490   // if (1 < t)
 491   //   return false;
 492   // with the original t. The multiplied test reads
 493   T absDenom = fabs(denom);
 494   if (absDenom < tDenom)
 495     return false;
 496
 497   // take the CPU accuracy in account
 498   T absDenomEps = absDenom*eps;
 499
 500   // T u = 1/denom*dot(p, s);
 501   T u = signDenom*dot(p, s);
 502   if (u < -absDenomEps)
 503     return false;
 504   // T v = 1/denom*dot(q, d);
 505   // if (v < -eps)
 506   //   return false;
 507   T v = signDenom*dot(q, lineSegment.getDirection());
 508   if (v < -absDenomEps)
 509     return false;
 510
 511   if (u + v > absDenom + absDenomEps)
 512     return false;
 513
 514   // return if paralell ??? FIXME what if paralell and in plane?
 515   // may be we are ok below than anyway??
 516   if (absDenom <= SGLimits<T>::min())
 517     return false;
 518
 519   x = lineSegment.getStart();
 520   // if we have survived here it could only happen with denom == 0
 521   // that the point is already in plane. Then return the origin ...
 522   if (SGLimitsd::min() < absDenom)
 523     x += (tDenom/absDenom)*lineSegment.getDirection();
 524
 525   return true;
 526 }
 527
 528 template<typename T>
 529 inline bool
 530 intersects(const SGTriangle<T>& tri, const SGLineSegment<T>& lineSegment, T eps = 0)
 531 {
 532   // FIXME: for now just wrap the other method. When that has prooven
 533   // well optimized, implement that special case
 534   SGVec3<T> dummy;
 535   return intersects(dummy, tri, lineSegment, eps);
 536 }
 537
 538
 539 template<typename T>
 540 inline SGVec3<T>
 541 closestPoint(const SGTriangle<T>& tri, const SGVec3<T>& p)
 542 {
 543   // This method minimizes the distance function Q(u, v) = || p - x ||
 544   // where x is a point in the trialgle given by the vertices v_i
 545   //  x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
 546   // The theoretical analysis is somehow too long for a comment.
 547   // May be it is sufficient to see that this code passes all the tests.
 548
 549   SGVec3<T> off = tri.getBaseVertex() - p;
 550   T a = dot(tri.getEdge(0), tri.getEdge(0));
 551   T b = dot(tri.getEdge(0), tri.getEdge(1));
 552   T c = dot(tri.getEdge(1), tri.getEdge(1));
 553   T d = dot(tri.getEdge(0), off);
 554   T e = dot(tri.getEdge(1), off);
 555
 556   T det = a*c - b*b;
 557
 558   T u = b*e - c*d;
 559   T v = b*d - a*e;
 560
 561 /*
 562   // Regions
 563   // \2|
 564   //  \|
 565   //   |\
 566   // 3 |0\ 1
 567   //----------
 568   // 4 | 5 \ 6
 569 */
 570
 571   if (u + v <= det) {
 572     if (u < 0) {
 573       if (v < 0) {
 574         // region 4
 575         if (d < 0) {
 576           if (a <= -d) {
 577             // u = 1;
 578             // v = 0;
 579             return tri.getBaseVertex() + tri.getEdge(0);
 580           } else {
 581             u = -d/a;
 582             // v = 0;
 583             return tri.getBaseVertex() + u*tri.getEdge(0);
 584           }
 585         } else {
 586           if (0 < e) {
 587             // u = 0;
 588             // v = 0;
 589             return tri.getBaseVertex();
 590           } else if (c <= -e) {
 591             // u = 0;
 592             // v = 1;
 593             return tri.getBaseVertex() + tri.getEdge(1);
 594           } else {
 595             // u = 0;
 596             v = -e/c;
 597             return tri.getBaseVertex() + v*tri.getEdge(1);
 598           }
 599         }
 600       } else {
 601         // region 3
 602         if (0 <= e) {
 603           // u = 0;
 604           // v = 0;
 605           return tri.getBaseVertex();
 606         } else if (c <= -e) {
 607           // u = 0;
 608           // v = 1;
 609           return tri.getBaseVertex() + tri.getEdge(1);
 610         } else {
 611           // u = 0;
 612           v = -e/c;
 613           return tri.getBaseVertex() + v*tri.getEdge(1);
 614         }
 615       }
 616     } else if (v < 0) {
 617       // region 5
 618       if (0 <= d) {
 619         // u = 0;
 620         // v = 0;
 621         return tri.getBaseVertex();
 622       } else if (a <= -d) {
 623         // u = 1;
 624         // v = 0;
 625         return tri.getBaseVertex() + tri.getEdge(0);
 626       } else {
 627         u = -d/a;
 628         // v = 0;
 629         return tri.getBaseVertex() + u*tri.getEdge(0);
 630       }
 631     } else {
 632       // region 0
 633       if (det <= SGLimits<T>::min()) {
 634         u = 0;
 635         v = 0;
 636         return tri.getBaseVertex();
 637       } else {
 638         T invDet = 1/det;
 639         u *= invDet;
 640         v *= invDet;
 641         return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
 642       }
 643     }
 644   } else {
 645     if (u < 0) {
 646       // region 2
 647       T tmp0 = b + d;
 648       T tmp1 = c + e;
 649       if (tmp0 < tmp1) {
 650         T numer = tmp1 - tmp0;
 651         T denom = a - 2*b + c;
 652         if (denom <= numer) {
 653           // u = 1;
 654           // v = 0;
 655           return tri.getBaseVertex() + tri.getEdge(0);
 656         } else {
 657           u = numer/denom;
 658           v = 1 - u;
 659           return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
 660         }
 661       } else {
 662         if (tmp1 <= 0) {
 663           // u = 0;
 664           // v = 1;
 665           return tri.getBaseVertex() + tri.getEdge(1);
 666         } else if (0 <= e) {
 667           // u = 0;
 668           // v = 0;
 669           return tri.getBaseVertex();
 670         } else {
 671           // u = 0;
 672           v = -e/c;
 673           return tri.getBaseVertex() + v*tri.getEdge(1);
 674         }
 675       }
 676     } else if (v < 0) {
 677       // region 6
 678       T tmp0 = b + e;
 679       T tmp1 = a + d;
 680       if (tmp0 < tmp1) {
 681         T numer = tmp1 - tmp0;
 682         T denom = a - 2*b + c;
 683         if (denom <= numer) {
 684           // u = 0;
 685           // v = 1;
 686           return tri.getBaseVertex() + tri.getEdge(1);
 687         } else {
 688           v = numer/denom;
 689           u = 1 - v;
 690           return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
 691         }
 692       } else {
 693         if (tmp1 < 0) {
 694           // u = 1;
 695           // v = 0;
 696           return tri.getBaseVertex() + tri.getEdge(0);
 697         } else if (0 <= d) {
 698           // u = 0;
 699           // v = 0;
 700           return tri.getBaseVertex();
 701         } else {
 702           u = -d/a;
 703           // v = 0;
 704           return tri.getBaseVertex() + u*tri.getEdge(0);
 705         }
 706       }
 707     } else {
 708       // region 1
 709       T numer = c + e - b - d;
 710       if (numer <= 0) {
 711         // u = 0;
 712         // v = 1;
 713         return tri.getVertex(2);
 714       } else {
 715         T denom = a - 2*b + c;
 716         if (denom <= numer) {
 717           // u = 1;
 718           // v = 0;
 719           return tri.getBaseVertex() + tri.getEdge(0);
 720         } else {
 721           u = numer/denom;
 722           v = 1 - u;
 723           return tri.getBaseVertex() + u*tri.getEdge(0) + v*tri.getEdge(1);
 724         }
 725       }
 726     }
 727   }
 728 }
 729 template<typename T>
 730 inline SGVec3<T>
 731 closestPoint(const SGVec3<T>& p, const SGTriangle<T>& tri)
 732 { return closestPoint(tri, p); }
 733
 734 template<typename T, typename T2>
 735 inline bool
 736 intersects(const SGTriangle<T>& tri, const SGSphere<T2>& sphere)
 737 {
 738   // This method minimizes the distance function Q(u, v) = || p - x ||
 739   // where x is a point in the trialgle given by the vertices v_i
 740   //  x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
 741   // The theoretical analysis is somehow too long for a comment.
 742   // May be it is sufficient to see that this code passes all the tests.
 743
 744   SGVec3<T> off = tri.getBaseVertex() - SGVec3<T>(sphere.getCenter());
 745   T baseDist2 = dot(off, off);
 746   T a = dot(tri.getEdge(0), tri.getEdge(0));
 747   T b = dot(tri.getEdge(0), tri.getEdge(1));
 748   T c = dot(tri.getEdge(1), tri.getEdge(1));
 749   T d = dot(tri.getEdge(0), off);
 750   T e = dot(tri.getEdge(1), off);
 751
 752   T det = a*c - b*b;
 753
 754   T u = b*e - c*d;
 755   T v = b*d - a*e;
 756
 757 /*
 758   // Regions
 759   // \2|
 760   //  \|
 761   //   |\
 762   // 3 |0\ 1
 763   //----------
 764   // 4 | 5 \ 6
 765 */
 766
 767   if (u + v <= det) {
 768     if (u < 0) {
 769       if (v < 0) {
 770         // region 4
 771         if (d < 0) {
 772           if (a <= -d) {
 773             // u = 1;
 774             // v = 0;
 775             T sqrDist = a + 2*d + baseDist2;
 776             return sqrDist <= sphere.getRadius2();
 777           } else {
 778             u = -d/a;
 779             // v = 0;
 780             T sqrDist = d*u + baseDist2;
 781             return sqrDist <= sphere.getRadius2();
 782           }
 783         } else {
 784           if (0 < e) {
 785             // u = 0;
 786             // v = 0;
 787             return baseDist2 <= sphere.getRadius2();
 788           } else if (c <= -e) {
 789             // u = 0;
 790             // v = 1;
 791             T sqrDist = c + 2*e + baseDist2;
 792             return sqrDist <= sphere.getRadius2();
 793           } else {
 794             // u = 0;
 795             v = -e/c;
 796             T sqrDist = e*v + baseDist2;
 797             return sqrDist <= sphere.getRadius2();
 798           }
 799         }
 800       } else {
 801         // region 3
 802         if (0 <= e) {
 803           // u = 0;
 804           // v = 0;
 805           return baseDist2 <= sphere.getRadius2();
 806         } else if (c <= -e) {
 807           // u = 0;
 808           // v = 1;
 809           T sqrDist = c + 2*e + baseDist2;
 810           return sqrDist <= sphere.getRadius2();
 811         } else {
 812           // u = 0;
 813           v = -e/c;
 814           T sqrDist = e*v + baseDist2;
 815           return sqrDist <= sphere.getRadius2();
 816         }
 817       }
 818     } else if (v < 0) {
 819       // region 5
 820       if (0 <= d) {
 821         // u = 0;
 822         // v = 0;
 823         return baseDist2 <= sphere.getRadius2();
 824       } else if (a <= -d) {
 825         // u = 1;
 826         // v = 0;
 827         T sqrDist = a + 2*d + baseDist2;
 828         return sqrDist <= sphere.getRadius2();
 829       } else {
 830         u = -d/a;
 831         // v = 0;
 832         T sqrDist = d*u + baseDist2;
 833         return sqrDist <= sphere.getRadius2();
 834       }
 835     } else {
 836       // region 0
 837       if (det <= SGLimits<T>::min()) {
 838         // sqrDist = baseDist2;
 839         u = 0;
 840         v = 0;
 841         return baseDist2 <= sphere.getRadius2();
 842       } else {
 843         T invDet = 1/det;
 844         u *= invDet;
 845         v *= invDet;
 846         T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e) + baseDist2;
 847         return sqrDist <= sphere.getRadius2();
 848       }
 849     }
 850   } else {
 851     if (u < 0) {
 852       // region 2
 853       T tmp0 = b + d;
 854       T tmp1 = c + e;
 855       if (tmp0 < tmp1) {
 856         T numer = tmp1 - tmp0;
 857         T denom = a - 2*b + c;
 858         if (denom <= numer) {
 859           // u = 1;
 860           // v = 0;
 861           T sqrDist = a + 2*d + baseDist2;
 862           return sqrDist <= sphere.getRadius2();
 863         } else {
 864           u = numer/denom;
 865           v = 1 - u;
 866           T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e) + baseDist2;
 867           return sqrDist <= sphere.getRadius2();
 868         }
 869       } else {
 870         if (tmp1 <= 0) {
 871           // u = 0;
 872           // v = 1;
 873           T sqrDist = c + 2*e + baseDist2;
 874           return sqrDist <= sphere.getRadius2();
 875         } else if (0 <= e) {
 876           // u = 0;
 877           // v = 0;
 878           return baseDist2 <= sphere.getRadius2();
 879         } else {
 880           // u = 0;
 881           v = -e/c;
 882           T sqrDist = e*v + baseDist2;
 883           return sqrDist <= sphere.getRadius2();
 884         }
 885       }
 886     } else if (v < 0) {
 887       // region 6
 888       T tmp0 = b + e;
 889       T tmp1 = a + d;
 890       if (tmp0 < tmp1) {
 891         T numer = tmp1 - tmp0;
 892         T denom = a - 2*b + c;
 893         if (denom <= numer) {
 894           // u = 0;
 895           // v = 1;
 896           T sqrDist = c + 2*e + baseDist2;
 897           return sqrDist <= sphere.getRadius2();
 898         } else {
 899           v = numer/denom;
 900           u = 1 - v;
 901           T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e)+baseDist2;
 902           return sqrDist <= sphere.getRadius2();
 903         }
 904       } else {
 905         if (tmp1 < 0) {
 906           // u = 1;
 907           // v = 0;
 908           T sqrDist = a + 2*d + baseDist2;
 909           return sqrDist <= sphere.getRadius2();
 910         } else if (0 <= d) {
 911           // sqrDist = baseDist2;
 912           // u = 0;
 913           // v = 0;
 914           return baseDist2 <= sphere.getRadius2();
 915         } else {
 916           u = -d/a;
 917           // v = 0;
 918           T sqrDist = d*u + baseDist2;
 919           return sqrDist <= sphere.getRadius2();
 920         }
 921       }
 922     } else {
 923       // region 1
 924       T numer = c + e - b - d;
 925       if (numer <= 0) {
 926         // u = 0;
 927         // v = 1;
 928         T sqrDist = c + 2*e + baseDist2;
 929         return sqrDist <= sphere.getRadius2();
 930       } else {
 931         T denom = a - 2*b + c;
 932         if (denom <= numer) {
 933           // u = 1;
 934           // v = 0;
 935           T sqrDist = a + 2*d + baseDist2;
 936           return sqrDist <= sphere.getRadius2();
 937         } else {
 938           u = numer/denom;
 939           v = 1 - u;
 940           T sqrDist = u*(a*u + b*v + 2*d) + v*(b*u + c*v + 2*e) + baseDist2;
 941           return sqrDist <= sphere.getRadius2();
 942         }
 943       }
 944     }
 945   }
 946 }
 947 template<typename T1, typename T2>
 948 inline bool
 949 intersects(const SGSphere<T1>& sphere, const SGTriangle<T2>& tri)
 950 { return intersects(tri, sphere); }
 951
 952
 953 template<typename T>
 954 inline bool
 955 intersects(const SGVec3<T>& v, const SGSphere<T>& sphere)
 956 {
 957   if (sphere.empty())
 958     return false;
 959   return distSqr(v, sphere.getCenter()) <= sphere.getRadius2();
 960 }
 961 template<typename T>
 962 inline bool
 963 intersects(const SGSphere<T>& sphere, const SGVec3<T>& v)
 964 { return intersects(v, sphere); }
 965
 966
 967 template<typename T>
 968 inline bool
 969 intersects(const SGBox<T>& box, const SGLineSegment<T>& lineSegment)
 970 {
 971   // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
 972
 973   SGVec3<T> c = lineSegment.getCenter() - box.getCenter();
 974   SGVec3<T> w = T(0.5)*lineSegment.getDirection();
 975   SGVec3<T> v(fabs(w.x()), fabs(w.y()), fabs(w.z()));
 976   SGVec3<T> h = T(0.5)*box.getSize();
 977
 978   if (fabs(c[0]) > v[0] + h[0])
 979     return false;
 980   if (fabs(c[1]) > v[1] + h[1])
 981     return false;
 982   if (fabs(c[2]) > v[2] + h[2])
 983     return false;
 984
 985   if (fabs(c[1]*w[2] - c[2]*w[1]) > h[1]*v[2] + h[2]*v[1])
 986     return false;
 987   if (fabs(c[0]*w[2] - c[2]*w[0]) > h[0]*v[2] + h[2]*v[0])
 988     return false;
 989   if (fabs(c[0]*w[1] - c[1]*w[0]) > h[0]*v[1] + h[1]*v[0])
 990     return false;
 991
 992   return true;
 993 }
 994 template<typename T>
 995 inline bool
 996 intersects(const SGLineSegment<T>& lineSegment, const SGBox<T>& box)
 997 { return intersects(box, lineSegment); }
 998
 999 template<typename T>
1000 inline bool
1001 intersects(const SGBox<T>& box, const SGRay<T>& ray)
1002 {
1003   // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
1004
1005   for (unsigned i = 0; i < 3; ++i) {
1006     T cMin = box.getMin()[i];
1007     T cMax = box.getMax()[i];
1008
1009     T cOrigin = ray.getOrigin()[i];
1010
1011     T cDir = ray.getDirection()[i];
1012     if (fabs(cDir) <= SGLimits<T>::min()) {
1013       if (cOrigin < cMin)
1014         return false;
1015       if (cMax < cOrigin)
1016         return false;
1017     }
1018
1019     T nearr = - SGLimits<T>::max();
1020     T farr = SGLimits<T>::max();
1021
1022     T T1 = (cMin - cOrigin) / cDir;
1023     T T2 = (cMax - cOrigin) / cDir;
1024     if (T1 > T2) std::swap (T1, T2);/* since T1 intersection with near plane */
1025     if (T1 > nearr) nearr = T1; /* want largest Tnear */
1026     if (T2 < farr) farr = T2; /* want smallest Tfarr */
1027     if (nearr > farr) // farr box is missed
1028       return false;
1029     if (farr < 0) // box is behind ray
1030       return false;
1031   }
1032
1033   return true;
1034 }
1035 // make it symmetric
1036 template<typename T>
1037 inline bool
1038 intersects(const SGRay<T>& ray, const SGBox<T>& box)
1039 { return intersects(box, ray); }
1040
1041 template<typename T1, typename T2>
1042 inline bool
1043 intersects(const SGBox<T1>& box1, const SGBox<T2>& box2)
1044 {
1045   if (box2.getMax()[0] < box1.getMin()[0])
1046     return false;
1047   if (box1.getMax()[0] < box2.getMin()[0])
1048     return false;
1049
1050   if (box2.getMax()[1] < box1.getMin()[1])
1051     return false;
1052   if (box1.getMax()[1] < box2.getMin()[1])
1053     return false;
1054
1055   if (box2.getMax()[2] < box1.getMin()[2])
1056     return false;
1057   if (box1.getMax()[2] < box2.getMin()[2])
1058     return false;
1059
1060   return true;
1061 }
1062
1063 #endif