simgear/math/SGIntersect.hxx

   1 // Copyright (C) 2006  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 SGBox<T>& box, const SGSphere<T>& sphere)
  24 {
  25   if (sphere.empty())
  26     return false;
  27   // Is more or less trivially included in the next tests
  28   // if (box.empty())
  29   //   return false;
  30
  31   if (sphere.getCenter().x() < box.getMin().x() - sphere.getRadius())
  32     return false;
  33   if (sphere.getCenter().y() < box.getMin().y() - sphere.getRadius())
  34     return false;
  35   if (sphere.getCenter().z() < box.getMin().z() - sphere.getRadius())
  36     return false;
  37
  38   if (box.getMax().x() + sphere.getRadius() < sphere.getCenter().x())
  39     return false;
  40   if (box.getMax().y() + sphere.getRadius() < sphere.getCenter().y())
  41     return false;
  42   if (box.getMax().z() + sphere.getRadius() < sphere.getCenter().z())
  43     return false;
  44
  45   return true;
  46 }
  47 // make it symmetric
  48 template<typename T>
  49 inline bool
  50 intersects(const SGSphere<T>& sphere, const SGBox<T>& box)
  51 { return intersects(box, sphere); }
  52
  53
  54 template<typename T>
  55 inline bool
  56 intersects(const SGVec3<T>& v, const SGBox<T>& 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 T>
  73 inline bool
  74 intersects(const SGBox<T>& box, const SGVec3<T>& 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 template<typename T>
 291 inline bool
 292 intersects(const SGRay<T>& ray, const SGSphere<T>& sphere)
 293 {
 294   // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering,
 295   // second edition, page 571
 296   SGVec3<T> l = sphere.getCenter() - ray.getOrigin();
 297   T s = dot(l, ray.getDirection());
 298   T l2 = dot(l, l);
 299
 300   T r2 = sphere.getRadius2();
 301   if (s < 0 && l2 > r2)
 302     return false;
 303
 304   T d2 = dot(ray.getDirection(), ray.getDirection());
 305   // The original test would read
 306   //   T m2 = l2 - s*s/d2;
 307   //   if (m2 > r2)
 308   //     return false;
 309   // but to avoid the expensive division, we multiply by d2
 310   T m2 = d2*l2 - s*s;
 311   if (m2 > d2*r2)
 312     return false;
 313
 314   return true;
 315 }
 316 // make it symmetric
 317 template<typename T>
 318 inline bool
 319 intersects(const SGSphere<T>& sphere, const SGRay<T>& ray)
 320 { return intersects(ray, sphere); }
 321
 322 template<typename T>
 323 inline bool
 324 intersects(const SGLineSegment<T>& lineSegment, 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() - lineSegment.getStart();
 329   T ld = length(lineSegment.getDirection());
 330   T s = dot(l, lineSegment.getDirection())/ld;
 331   T l2 = dot(l, l);
 332
 333   T r2 = sphere.getRadius2();
 334   if (s < 0 && l2 > r2)
 335     return false;
 336
 337   T m2 = l2 - s*s;
 338   if (m2 > r2)
 339     return false;
 340
 341   T q = sqrt(r2 - m2);
 342   T t = s - q;
 343   if (ld < t)
 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 SGLineSegment<T>& lineSegment)
 352 { return intersects(lineSegment, sphere); }
 353
 354
 355 template<typename T>
 356 inline bool
 357 // FIXME do not use that default argument later. Just for development now
 358 intersects(SGVec3<T>& x, const SGTriangle<T>& tri, const SGRay<T>& ray, T eps = 0)
 359 {
 360   // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
 361
 362   // Method based on the observation that we are looking for a
 363   // point x that can be expressed in terms of the triangle points
 364   //  x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
 365   // with 0 <= u, v and u + v <= 1.
 366   // OTOH it could be expressed in terms of the ray
 367   //  x = o + t*d
 368   // Now we can compute u, v and t.
 369   SGVec3<T> p = cross(ray.getDirection(), tri.getEdge(1));
 370
 371   T denom = dot(p, tri.getEdge(0));
 372   T signDenom = copysign(1, denom);
 373
 374   SGVec3<T> s = ray.getOrigin() - tri.getBaseVertex();
 375   SGVec3<T> q = cross(s, tri.getEdge(0));
 376   // Now t would read
 377   //   t = 1/denom*dot(q, tri.getEdge(1));
 378   // To avoid an expensive division we multiply by |denom|
 379   T tDenom = signDenom*dot(q, tri.getEdge(1));
 380   if (tDenom < 0)
 381     return false;
 382   // For line segment we would test against
 383   // if (1 < t)
 384   //   return false;
 385   // with the original t. The multiplied test would read
 386   // if (absDenom < tDenom)
 387   //   return false;
 388
 389   T absDenom = fabs(denom);
 390   T absDenomEps = absDenom*eps;
 391
 392   // T u = 1/denom*dot(p, s);
 393   T u = signDenom*dot(p, s);
 394   if (u < -absDenomEps)
 395     return false;
 396   // T v = 1/denom*dot(q, d);
 397   // if (v < -eps)
 398   //   return false;
 399   T v = signDenom*dot(q, ray.getDirection());
 400   if (v < -absDenomEps)
 401     return false;
 402
 403   if (u + v > absDenom + absDenomEps)
 404     return false;
 405
 406   // return if paralell ??? FIXME what if paralell and in plane?
 407   // may be we are ok below than anyway??
 408   if (absDenom <= SGLimits<T>::min())
 409     return false;
 410
 411   x = ray.getOrigin();
 412   // if we have survived here it could only happen with denom == 0
 413   // that the point is already in plane. Then return the origin ...
 414   if (SGLimitsd::min() < absDenom)
 415     x += (tDenom/absDenom)*ray.getDirection();
 416
 417   return true;
 418 }
 419
 420 template<typename T>
 421 inline bool
 422 intersects(const SGTriangle<T>& tri, const SGRay<T>& ray, T eps = 0)
 423 {
 424   // FIXME: for now just wrap the other method. When that has prooven
 425   // well optimized, implement that special case
 426   SGVec3<T> dummy;
 427   return intersects(dummy, tri, ray, eps);
 428 }
 429
 430 template<typename T>
 431 inline bool
 432 // FIXME do not use that default argument later. Just for development now
 433 intersects(SGVec3<T>& x, const SGTriangle<T>& tri, const SGLineSegment<T>& lineSegment, T eps = 0)
 434 {
 435   // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
 436
 437   // Method based on the observation that we are looking for a
 438   // point x that can be expressed in terms of the triangle points
 439   //  x = v_0 + u*(v_1 - v_0) + v*(v_2 - v_0)
 440   // with 0 <= u, v and u + v <= 1.
 441   // OTOH it could be expressed in terms of the lineSegment
 442   //  x = o + t*d
 443   // Now we can compute u, v and t.
 444   SGVec3<T> p = cross(lineSegment.getDirection(), tri.getEdge(1));
 445
 446   T denom = dot(p, tri.getEdge(0));
 447   T signDenom = copysign(1, denom);
 448
 449   SGVec3<T> s = lineSegment.getStart() - tri.getBaseVertex();
 450   SGVec3<T> q = cross(s, tri.getEdge(0));
 451   // Now t would read
 452   //   t = 1/denom*dot(q, tri.getEdge(1));
 453   // To avoid an expensive division we multiply by |denom|
 454   T tDenom = signDenom*dot(q, tri.getEdge(1));
 455   if (tDenom < 0)
 456     return false;
 457   // For line segment we would test against
 458   // if (1 < t)
 459   //   return false;
 460   // with the original t. The multiplied test reads
 461   T absDenom = fabs(denom);
 462   if (absDenom < tDenom)
 463     return false;
 464
 465   // take the CPU accuracy in account
 466   T absDenomEps = absDenom*eps;
 467
 468   // T u = 1/denom*dot(p, s);
 469   T u = signDenom*dot(p, s);
 470   if (u < -absDenomEps)
 471     return false;
 472   // T v = 1/denom*dot(q, d);
 473   // if (v < -eps)
 474   //   return false;
 475   T v = signDenom*dot(q, lineSegment.getDirection());
 476   if (v < -absDenomEps)
 477     return false;
 478
 479   if (u + v > absDenom + absDenomEps)
 480     return false;
 481
 482   // return if paralell ??? FIXME what if paralell and in plane?
 483   // may be we are ok below than anyway??
 484   if (absDenom <= SGLimits<T>::min())
 485     return false;
 486
 487   x = lineSegment.getStart();
 488   // if we have survived here it could only happen with denom == 0
 489   // that the point is already in plane. Then return the origin ...
 490   if (SGLimitsd::min() < absDenom)
 491     x += (tDenom/absDenom)*lineSegment.getDirection();
 492
 493   return true;
 494 }
 495
 496 template<typename T>
 497 inline bool
 498 intersects(const SGTriangle<T>& tri, const SGLineSegment<T>& lineSegment, T eps = 0)
 499 {
 500   // FIXME: for now just wrap the othr method. When that has prooven
 501   // well optimized, implement that special case
 502   SGVec3<T> dummy;
 503   return intersects(dummy, tri, lineSegment, eps);
 504 }
 505
 506
 507 template<typename T>
 508 inline bool
 509 intersects(const SGVec3<T>& v, const SGSphere<T>& sphere)
 510 {
 511   if (sphere.empty())
 512     return false;
 513   return distSqr(v, sphere.getCenter()) <= sphere.getRadius2();
 514 }
 515 template<typename T>
 516 inline bool
 517 intersects(const SGSphere<T>& sphere, const SGVec3<T>& v)
 518 { return intersects(v, sphere); }
 519
 520
 521 template<typename T>
 522 inline bool
 523 intersects(const SGBox<T>& box, const SGLineSegment<T>& lineSegment)
 524 {
 525   // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
 526
 527   SGVec3<T> c = lineSegment.getCenter() - box.getCenter();
 528   SGVec3<T> w = 0.5*lineSegment.getDirection();
 529   SGVec3<T> v(fabs(w.x()), fabs(w.y()), fabs(w.z()));
 530   SGVec3<T> h = 0.5*box.getSize();
 531
 532   if (fabs(c[0]) > v[0] + h[0])
 533     return false;
 534   if (fabs(c[1]) > v[1] + h[1])
 535     return false;
 536   if (fabs(c[2]) > v[2] + h[2])
 537     return false;
 538
 539   if (fabs(c[1]*w[2] - c[2]*w[1]) > h[1]*v[2] + h[2]*v[1])
 540     return false;
 541   if (fabs(c[0]*w[2] - c[2]*w[0]) > h[0]*v[2] + h[2]*v[0])
 542     return false;
 543   if (fabs(c[0]*w[1] - c[1]*w[0]) > h[0]*v[1] + h[1]*v[0])
 544     return false;
 545
 546   return true;
 547 }
 548 template<typename T>
 549 inline bool
 550 intersects(const SGLineSegment<T>& lineSegment, const SGBox<T>& box)
 551 { return intersects(box, lineSegment); }
 552
 553 template<typename T>
 554 inline bool
 555 intersects(const SGBox<T>& box, const SGRay<T>& ray)
 556 {
 557   // See Tomas Akeniene - Moeller/Eric Haines: Real Time Rendering
 558
 559   for (unsigned i = 0; i < 3; ++i) {
 560     T cMin = box.getMin()[i];
 561     T cMax = box.getMax()[i];
 562
 563     T cOrigin = ray.getOrigin()[i];
 564
 565     T cDir = ray.getDirection()[i];
 566     if (fabs(cDir) <= SGLimits<T>::min()) {
 567       if (cOrigin < cMin)
 568         return false;
 569       if (cMax < cOrigin)
 570         return false;
 571     }
 572
 573     T near = - SGLimits<T>::max();
 574     T far = SGLimits<T>::max();
 575
 576     T T1 = (cMin - cOrigin) / cDir;
 577     T T2 = (cMax - cOrigin) / cDir;
 578     if (T1 > T2) std::swap (T1, T2);/* since T1 intersection with near plane */
 579     if (T1 > near) near = T1; /* want largest Tnear */
 580     if (T2 < far) far = T2; /* want smallest Tfar */
 581     if (near > far) // far box is missed
 582       return false;
 583     if (far < 0) // box is behind ray
 584       return false;
 585   }
 586
 587   return true;
 588 }
 589 // make it symmetric
 590 template<typename T>
 591 inline bool
 592 intersects(const SGRay<T>& ray, const SGBox<T>& box)
 593 { return intersects(box, ray); }
 594
 595 #endif