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