QuOc

 

geom.h

Go to the documentation of this file.
00001 /* Copyright (c) 2001-2010 AG Rumpf, INS, Universitaet Bonn                      */
00002 /*                                                                               */
00003 /* The contents of this file are subject to the terms of the Common Development  */
00004 /* and Distribution License Version 1.0 (the "License"); you may not use         */
00005 /* this file except in compliance with the License. You may obtain a copy of     */
00006 /* the License at http://www.sun.com/cddl/                                       */
00007 /*                                                                               */
00008 /* Software distributed under the License is distributed on an "AS IS" basis,    */
00009 /* WITHOUT WARRANTY OF ANY KIND, either express or implied.                      */
00010 /*                                                                               */
00011 #ifndef __GEOM_H
00012 #define __GEOM_H
00013 
00014 #include <aol.h>
00015 #include <quoc.h>
00016 #include <triangle.h>
00017 #include <smallMat.h>
00018 
00019 namespace aol {
00020 
00021 enum PROJ_TYPE {
00022   PT_INSIDE, P1, P2
00023 };
00024 
00025 template <class PRIMITIVE_A, class PRIMITIVE_B>
00026 class Intersector { };
00027 
00028 template <class RealType> class Plane;
00029 //template <class RealType> class Triang;
00030 
00031 
00032 template <typename RealType, qc::Dimension EmbedDim>
00033 class LineSegment { };
00034 
00035 template <typename RealType>
00036 class LineSegment<RealType, qc::QC_2D> {
00037   friend class Intersector<Plane<RealType>, Triangle<RealType> >;
00038 public:
00039   LineSegment(  ) {
00040     _pt1.setZero();
00041     _pt2.setZero();
00042   }
00043 
00044   LineSegment ( const aol::Vec2<RealType> &Pt1, const aol::Vec2<RealType> &Pt2 ) :
00045       _pt1 ( Pt1 ), _pt2 ( Pt2 ) {}
00046 
00047   void getBoundingBox ( aol::Vec2<RealType> &min, aol::Vec2<RealType> &max ) const {
00048     for ( int i = 0; i < 2; i++ ) {
00049       min[i] = aol::Min ( _pt1[i], _pt2[i] );
00050       max[i] = aol::Max ( _pt1[i], _pt2[i] );
00051     }
00052   }
00053 
00054 
00055   void getBoundingBox ( aol::Vec2<int> &min, aol::Vec2<int> &max ) const {
00056     aol::Vec2<RealType> rmin, rmax;
00057     getBoundingBox ( rmin, rmax );
00058     for ( int i = 0; i < 2; i++ ) {
00059       min[i] = static_cast<int> ( floor ( rmin[i] ) );
00060       max[i] = static_cast<int> ( ceil ( rmax[i] ) );
00061     }
00062   }
00063 
00064   /*** compute distance to linesegment */
00065   RealType dist ( const aol::Vec3<RealType> &Pt ) const {
00066     return dist ( aol::Vec2<RealType> ( Pt[0], Pt[1] ) );
00067   }
00068 
00069 
00070   /*** compute distance to linesegment */
00071   RealType dist ( const aol::Vec2<RealType> &Pt ) const {
00072     aol::Vec2<RealType> a, b;
00073     a = _pt2; a -= Pt;
00074     b = _pt1; b -= _pt2;
00075     RealType lambda = - ( a * b ) / ( b * b );
00076     if ( lambda >= 0. && lambda <= 1. ) {
00077       b *= lambda; a += b;
00078       return a.norm();
00079     } else {
00080       RealType d1, d2;
00081       a = _pt2; a -= Pt;
00082       d1 = a.norm();
00083       a = _pt1; a -= Pt;
00084       d2 = a.norm();
00085       return aol::Min ( d1, d2 );
00086     }
00087   }
00088 
00089   /*** compute distance to linesegment */
00090   PROJ_TYPE projectTo ( const aol::Vec2<RealType> &Pt, aol::Vec2<RealType> &ProjPt ) const {
00091     aol::Vec2<RealType> a, b;
00092     a = _pt2; a -= Pt;
00093     b = _pt1; b -= _pt2;
00094     RealType lambda = - ( a * b ) / ( b * b );
00095     if ( lambda >= 0. && lambda <= 1. ) {
00096       ProjPt = b;
00097       ProjPt *= lambda;
00098       ProjPt += _pt2;
00099       return PT_INSIDE;
00100     } else if ( lambda < 0. ) {
00101       ProjPt = _pt2;
00102       return P2;
00103     } else {
00104       ProjPt = _pt1;
00105       return P2;
00106     }
00107   }
00108 
00109   void normal ( aol::Vec2<RealType> &n ) const {
00110     for ( int c = 0; c < 2; c++ ) {
00111       n[c] = _pt2[c] - _pt1[c];
00112     }
00113     n /= sqrt ( n * n );
00114     RealType tmp = -n[0];
00115     n[0] = n[1];
00116     n[1] = tmp;
00117   }
00118 
00119   RealType length( ) const {
00120     return sqrt ( aol::Sqr ( _pt1[0] - _pt2[0] ) +
00121                   aol::Sqr ( _pt1[1] - _pt2[1] ) );
00122   }
00123 
00124   aol::Vec2<RealType>& beginPoint() { return _pt1; }
00125   aol::Vec2<RealType>& endPoint() { return _pt2; }
00126 
00127   const aol::Vec2<RealType>& beginPoint() const { return _pt1; }
00128   const aol::Vec2<RealType>& endPoint() const { return _pt2; }
00129 protected:
00130   aol::Vec2<RealType> _pt1, _pt2;
00131 };
00132 
00133 
00134 /**
00135  * utility class, representing a line segment, i.e. the connection between two points.
00136  * @ingroup Line
00137  */
00138 template <typename RealType>
00139 class LineSegment<RealType, qc::QC_3D> {
00140   friend class Intersector<Plane<RealType>, Triangle<RealType> >;
00141 public:
00142   LineSegment(  ) {
00143     _pt1.setZero();
00144     _pt2.setZero();
00145   }
00146 
00147   LineSegment ( const aol::Vec3<RealType> &Pt1, const aol::Vec3<RealType> &Pt2 ) :
00148       _pt1 ( Pt1 ), _pt2 ( Pt2 ) {}
00149 
00150   /*** compute distance to linesegment */
00151   RealType dist ( const aol::Vec3<RealType> &Pt ) {
00152     aol::Vec3<RealType> a, b;
00153     a = _pt2; a -= Pt;
00154     b = _pt1; b -= _pt2;
00155     RealType lambda = - ( a * b ) / ( b * b );
00156     if ( lambda >= 0. && lambda <= 1. ) {
00157       b *= lambda; a += b;
00158       return a.norm();
00159     } else {
00160       RealType d1, d2;
00161       a = _pt2; a -= Pt;
00162       d1 = a.norm();
00163       a = _pt1; a -= Pt;
00164       d2 = a.norm();
00165       return aol::Min ( d1, d2 );
00166     }
00167   }
00168 
00169   /*** compute distance to linesegment */
00170   void projectTo ( const aol::Vec3<RealType> &Pt, aol::Vec3<RealType> &ProjPt ) {
00171     aol::Vec3<RealType> a, b;
00172     a = _pt2; a -= Pt;
00173     b = _pt1; b -= _pt2;
00174     RealType lambda = - ( a * b ) / ( b * b );
00175     if ( lambda >= 0. && lambda <= 1. ) {
00176       ProjPt = b;
00177       ProjPt *= lambda;
00178       ProjPt += _pt2;
00179     } else if ( lambda < 0. ) {
00180       ProjPt = _pt2;
00181     } else {
00182       ProjPt = _pt1;
00183     }
00184   }
00185 
00186   RealType length( ) const {
00187     return sqrt ( aol::Sqr ( _pt1[0] - _pt2[0] ) +
00188                   aol::Sqr ( _pt1[1] - _pt2[1] ) +
00189                   aol::Sqr ( _pt1[2] - _pt2[2] ) );
00190   }
00191 
00192   aol::Vec3<RealType>& beginPoint() { return _pt1; }
00193   aol::Vec3<RealType>& endPoint() { return _pt2; }
00194 
00195   const aol::Vec3<RealType>& beginPoint() const { return _pt1; }
00196   const aol::Vec3<RealType>& endPoint() const { return _pt2; }
00197 protected:
00198   aol::Vec3<RealType> _pt1, _pt2;
00199 };
00200 
00201 template <class RealType>
00202 class Plane {
00203 public:
00204   aol::Vec3<RealType> _normal;
00205   RealType _alpha;
00206 
00207   Plane ( const aol::Vec3<RealType> &normal, RealType alpha )
00208       : _normal ( normal ), _alpha ( alpha ) {}
00209 };
00210 
00211 
00212 template <class RealType>  class AlignedCube {
00213 public:
00214   aol::Vec3<RealType> _min, _max;
00215 
00216   AlignedCube ( const aol::Vec3<RealType> &min, const aol::Vec3<RealType> &max )
00217       : _min ( min ), _max ( max ) {}
00218 
00219   AlignedCube ( const RealType minX,
00220                 const RealType minY,
00221                 const RealType minZ,
00222                 const RealType maxX,
00223                 const RealType maxY,
00224                 const RealType maxZ )
00225     : _min ( minX, minY, minZ ),
00226       _max ( maxX, maxY, maxZ ) {}
00227 };
00228 
00229 template <class RealType>  class AlignedQuad {
00230 public:
00231   aol::Vec2<RealType> _min, _max;
00232 
00233   AlignedQuad ( const aol::Vec2<RealType> &min, const aol::Vec2<RealType> &max )
00234       : _min ( min ), _max ( max ) {}
00235 
00236 };
00237 
00238 
00239 template <class RealType>
00240 class Intersector<Plane<RealType>, Triangle<RealType> > {
00241 public:
00242 
00243   // returns number of intersections of t's edges with p.
00244   // see ref/geom1.tex
00245   static int cut ( const Plane<RealType> &p, const Triangle<RealType> &t, LineSegment<RealType, qc::QC_3D> &s ) {
00246     aol::Vec3<RealType> d1, d2;
00247     d1 = t[0]; d1 -= t[2];
00248     d2 = t[1]; d2 -= t[2];
00249     const RealType beta = p._alpha - p._normal * t[2];
00250 
00251     const RealType mu1 = d1 * p._normal, mu2 = d2 * p._normal;
00252 
00253     int num = 0;
00254     aol::Vec3<RealType> *pt = & ( s._pt1 );
00255 
00256     // lambda1 = 0
00257     RealType lambda2 = beta / mu2;
00258     if ( mu2 != 0. && lambda2 >= 0. && lambda2 <= 1. ) {
00259       *pt = t[1];
00260       *pt -= t[2];
00261       *pt *= lambda2;
00262       *pt += t[2];
00263       num ++;
00264       if ( num == 1 ) {
00265         pt = & ( s._pt2 );
00266       }
00267     }
00268 
00269     // lambda2 = 0
00270     RealType lambda1 = beta / mu1;
00271     if ( mu1 != 0. && lambda1 >= 0. && lambda1 <= 1. ) {
00272       *pt = t[0];
00273       *pt -= t[2];
00274       *pt *= lambda1;
00275       *pt += t[2];
00276 
00277       num++;
00278       if ( num == 1 ) {
00279         pt = & ( s._pt2 );
00280       }
00281     }
00282 
00283     // lambda3 = 0
00284     lambda2 = ( beta - mu1 ) / ( mu2 - mu1 );
00285     if ( lambda2 >= 0. && lambda2 <= 1. ) {
00286       *pt = t[1];
00287       *pt -= t[0];
00288       *pt *= lambda2;
00289       *pt += t[0];
00290       num ++;
00291     }
00292 
00293     if ( num > 2 ) {
00294       cerr << "error: num > 2!\n";
00295     }
00296     return num;
00297   }
00298 };
00299 
00300 
00301 
00302 
00303 template <class RealType>
00304 class Intersector<LineSegment<RealType, qc::QC_2D>, LineSegment<RealType, qc::QC_2D> > {
00305 
00306 public:
00307   static int cut ( const LineSegment<RealType, qc::QC_2D> &s1,
00308                    const LineSegment<RealType, qc::QC_2D> &s2, aol::Vec2<RealType> &pt  ) {
00309 
00310     aol::Matrix22<RealType> mat, inv;
00311     aol::Vec2<RealType> d1, d2, d3;
00312     aol::Vec2<RealType> rhs, alpha;
00313 
00314     d1 = s1.beginPoint(); d1 -= s1.endPoint();
00315     d2 = s2.endPoint(); d2 -= s2.beginPoint();
00316     d3 = s1.endPoint(); d3 -= s2.endPoint();
00317 
00318     mat[0][0] = d1 * d1;
00319     mat[1][0] = mat[0][1] = d1 * d2;
00320     mat[1][1] = d2 * d2;
00321 
00322     rhs[0] = -d1 * d3;
00323     rhs[1] = -d2 * d3;
00324 
00325     try {
00326       mat.make_inverse ( inv );
00327     } catch ( aol::Exception &ex ) {
00328       cerr << mat
00329       << " " << s1.beginPoint() << " " << s1.endPoint()
00330       << " " << s2.beginPoint() << " " << s2.endPoint() << endl;
00331       ex.dump();
00332       return 0;
00333     }
00334     inv.mult ( rhs, alpha );
00335 
00336     if ( mat.det() == 0. ) {
00337       return 0;
00338       // TODO: this could also be infinity, i.e. overlapping of the two segments
00339     }
00340 
00341     pt = s1.beginPoint();
00342     pt -= s1.endPoint();
00343     pt *= alpha[0];
00344     pt += s1.endPoint();
00345 
00346     if ( alpha[0] >= 0. && alpha[0] <= 1. &&
00347          alpha[1] >= 0. && alpha[1] <= 1. ) {
00348       return 1;
00349     } else {
00350       return 0;
00351     }
00352 
00353   }
00354 
00355 };
00356 
00357 
00358 
00359 template <class RealType>
00360 class Intersector<LineSegment<RealType, qc::QC_3D>, LineSegment<RealType, qc::QC_3D> > {
00361 
00362 public:
00363   static int cut ( const LineSegment<RealType, qc::QC_3D> &s1,
00364                    const LineSegment<RealType, qc::QC_3D> &s2, aol::Vec3<RealType> &pt  ) {
00365 
00366     aol::Matrix22<RealType> mat, inv;
00367     aol::Vec3<RealType> d1, d2, d3;
00368     aol::Vec2<RealType> rhs, alpha;
00369 
00370     d1 = s1.beginPoint(); d1 -= s1.endPoint();
00371     d2 = s2.endPoint(); d2 -= s2.beginPoint();
00372     d3 = s1.endPoint(); d3 -= s2.endPoint();
00373 
00374     mat[0][0] = d1 * d1;
00375     mat[1][0] = mat[0][1] = d1 * d2;
00376     mat[1][1] = d2 * d2;
00377 
00378     rhs[0] = -d1 * d3;
00379     rhs[1] = -d2 * d3;
00380 
00381     try {
00382       mat.make_inverse ( inv );
00383     } catch ( aol::Exception &ex ) {
00384       cerr << mat
00385       << " " << s1.beginPoint() << " " << s1.endPoint()
00386       << " " << s2.beginPoint() << " " << s2.endPoint() << endl;
00387       ex.dump();
00388       return 0;
00389     }
00390     inv.mult ( rhs, alpha );
00391 
00392     if ( mat.det() == 0. ) {
00393       return 0;
00394       // TODO: this could also be infinity, i.e. overlapping of the two segments
00395     }
00396 
00397     pt = s1.beginPoint();
00398     pt -= s1.endPoint();
00399     pt *= alpha[0];
00400     pt += s1.endPoint();
00401 
00402     if ( alpha[0] >= 0. && alpha[0] <= 1. &&
00403          alpha[1] >= 0. && alpha[1] <= 1. ) {
00404       return 1;
00405     } else {
00406       return 0;
00407     }
00408 
00409   }
00410 
00411 };
00412 
00413 
00414 
00415 
00416 template <class RealType>
00417 class Intersector<AlignedCube<RealType>, Triangle<RealType> > {
00418 public:
00419 
00420   static bool has_intersection ( const AlignedCube<RealType> &c,
00421                                  const Triangle<RealType> &t )  {
00422     aol::Vec3<RealType> bbmin, bbmax;
00423     t.getBoundingBox ( bbmin, bbmax );
00424 
00425     bool completely_inside = true;
00426     for ( int i = 0; i < 3; i++ ) {
00427       if ( ! ( c._min[i] <= bbmin[i] &&
00428                c._max[i] >= bbmax[i] ) ) {
00429         completely_inside = false;
00430         break;
00431       }
00432     }
00433 
00434     if ( completely_inside ) {
00435       //cerr << "completely inside: "
00436       //   << c._min << " " << c._max << " "
00437       //   << bbmin << " " << bbmax << " " << endl;
00438 
00439       return true;
00440     }
00441 
00442 
00443     if ( bbmin[0] <= c._min[0] && bbmax[0] >= c._min[0] ) {
00444       Plane<RealType> p ( aol::Vec3<RealType> ( 1., 0., 0. ), c._min[0] );
00445       aol::LineSegment<RealType, qc::QC_3D> seg;
00446       int num = Intersector<Plane<RealType>, Triangle<RealType> >::cut ( p, t, seg );
00447 
00448       aol::Vec2<RealType> min2d ( c._min[1], c._min[2] );
00449       aol::Vec2<RealType> max2d ( c._max[1], c._max[2] );
00450       aol::Vec2<RealType> seg2d1 ( seg.beginPoint() [1], seg.beginPoint() [2] );
00451       aol::Vec2<RealType> seg2d2 ( seg.endPoint() [1], seg.endPoint() [2] );
00452 
00453       if ( num == 2 &&
00454            ( is_inside ( min2d, max2d, seg2d1 ) ||
00455              is_inside ( min2d, max2d, seg2d2 ) ||
00456              rect_segment_intersect ( min2d, max2d, seg2d1, seg2d2 ) ) ) {
00457         return true;
00458       }
00459     }
00460 
00461     if ( bbmin[0] <= c._max[0] && bbmax[0] >= c._max[0] ) {
00462       Plane<RealType> p ( aol::Vec3<RealType> ( 1., 0., 0. ), c._max[0] );
00463       aol::LineSegment<RealType, qc::QC_3D> seg;
00464       int num = Intersector<Plane<RealType>, Triangle<RealType> >::cut ( p, t, seg );
00465 
00466       aol::Vec2<RealType> min2d ( c._min[1], c._min[2] );
00467       aol::Vec2<RealType> max2d ( c._max[1], c._max[2] );
00468       aol::Vec2<RealType> seg2d1 ( seg.beginPoint() [1], seg.beginPoint() [2] );
00469       aol::Vec2<RealType> seg2d2 ( seg.endPoint  () [1], seg.endPoint  () [2] );
00470 
00471       if ( num == 2 &&
00472            ( is_inside ( min2d, max2d, seg2d1 ) ||
00473              is_inside ( min2d, max2d, seg2d2 ) ||
00474              rect_segment_intersect ( min2d, max2d, seg2d1, seg2d2 ) ) ) {
00475         return true;
00476       }
00477     }
00478 
00479 
00480     if ( bbmin[1] <= c._min[1] && bbmax[1] >= c._min[1] ) {
00481       Plane<RealType> p ( aol::Vec3<RealType> ( 0., 1., 0. ), c._min[1] );
00482       aol::LineSegment<RealType, qc::QC_3D> seg;
00483       int num = Intersector<Plane<RealType>, Triangle<RealType> >::cut ( p, t, seg );
00484 
00485       aol::Vec2<RealType> min2d ( c._min[0], c._min[2] );
00486       aol::Vec2<RealType> max2d ( c._max[0], c._max[2] );
00487       aol::Vec2<RealType> seg2d1 ( seg.beginPoint() [0], seg.beginPoint() [2] );
00488       aol::Vec2<RealType> seg2d2 ( seg.endPoint() [0], seg.endPoint() [2] );
00489 
00490       if ( num == 2 &&
00491            ( is_inside ( min2d, max2d, seg2d1 ) ||
00492              is_inside ( min2d, max2d, seg2d2 ) ||
00493              rect_segment_intersect ( min2d, max2d, seg2d1, seg2d2 ) ) ) {
00494         return true;
00495       }
00496     }
00497 
00498 
00499     if ( bbmin[1] <= c._max[1] && bbmax[1] >= c._max[1] ) {
00500       Plane<RealType> p ( aol::Vec3<RealType> ( 0., 1., 0. ), c._max[1] );
00501       aol::LineSegment<RealType, qc::QC_3D> seg;
00502       int num = Intersector<Plane<RealType>, Triangle<RealType> >::cut ( p, t, seg );
00503 
00504       aol::Vec2<RealType> min2d ( c._min[0], c._min[2] );
00505       aol::Vec2<RealType> max2d ( c._max[0], c._max[2] );
00506       aol::Vec2<RealType> seg2d1 ( seg.beginPoint() [0], seg.beginPoint() [2] );
00507       aol::Vec2<RealType> seg2d2 ( seg.endPoint() [0], seg.endPoint() [2] );
00508 
00509       if ( num == 2 &&
00510            ( is_inside ( min2d, max2d, seg2d1 ) ||
00511              is_inside ( min2d, max2d, seg2d2 ) ||
00512              rect_segment_intersect ( min2d, max2d, seg2d1, seg2d2 ) ) ) {
00513         return true;
00514       }
00515     }
00516 
00517 
00518     if ( bbmin[2] <= c._min[2] && bbmax[2] >= c._min[2] ) {
00519       Plane<RealType> p ( aol::Vec3<RealType> ( 0., 0., 1. ), c._min[2] );
00520       aol::LineSegment<RealType, qc::QC_3D> seg;
00521       int num = Intersector<Plane<RealType>, Triangle<RealType> >::cut ( p, t, seg );
00522 
00523       aol::Vec2<RealType> min2d ( c._min[0], c._min[1] );
00524       aol::Vec2<RealType> max2d ( c._max[0], c._max[1] );
00525       aol::Vec2<RealType> seg2d1 ( seg.beginPoint() [0], seg.beginPoint() [1] );
00526       aol::Vec2<RealType> seg2d2 ( seg.endPoint() [0], seg.endPoint() [1] );
00527 
00528       if ( num == 2 &&
00529            ( is_inside ( min2d, max2d, seg2d1 ) ||
00530              is_inside ( min2d, max2d, seg2d2 ) ||
00531              rect_segment_intersect ( min2d, max2d, seg2d1, seg2d2 ) ) ) {
00532         return true;
00533       }
00534     }
00535 
00536     if ( bbmin[2] <= c._max[2] && bbmax[2] >= c._max[2] ) {
00537       Plane<RealType> p ( aol::Vec3<RealType> ( 0., 0., 1. ), c._max[2] );
00538       aol::LineSegment<RealType, qc::QC_3D> seg;
00539       int num = Intersector<Plane<RealType>, Triangle<RealType> >::cut ( p, t, seg );
00540 
00541       aol::Vec2<RealType> min2d ( c._min[0], c._min[1] );
00542       aol::Vec2<RealType> max2d ( c._max[0], c._max[1] );
00543       aol::Vec2<RealType> seg2d1 ( seg.beginPoint() [0], seg.beginPoint() [1] );
00544       aol::Vec2<RealType> seg2d2 ( seg.endPoint() [0], seg.endPoint() [1] );
00545 
00546       if ( num == 2 &&
00547            ( is_inside ( min2d, max2d, seg2d1 ) ||
00548              is_inside ( min2d, max2d, seg2d2 ) ||
00549              rect_segment_intersect ( min2d, max2d, seg2d1, seg2d2 ) ) ) {
00550         return true;
00551       }
00552     }
00553     return false;
00554   }
00555 
00556   inline static bool is_inside ( const aol::Vec2<RealType> &min, const aol::Vec2<RealType> &max,
00557                                  const aol::Vec2<RealType> &p ) {
00558     return ( min[0] <= p[0] && min[1] <= p[1] && p[0] <= max[0] && p[1] <= max[1] );
00559   }
00560 
00561   inline static bool rect_segment_intersect ( const aol::Vec2<RealType> &min, const aol::Vec2<RealType> &max,
00562                                               const aol::Vec2<RealType> &p1, const aol::Vec2<RealType> &p2 ) {
00563     return ( segments_intersect ( p1, p2, aol::Vec2<RealType> ( min[0], min[1] ),  aol::Vec2<RealType> ( min[0], max[1] ) ) ||
00564              segments_intersect ( p1, p2, aol::Vec2<RealType> ( min[0], min[1] ),  aol::Vec2<RealType> ( max[0], min[1] ) ) ||
00565              segments_intersect ( p1, p2, aol::Vec2<RealType> ( min[0], max[1] ),  aol::Vec2<RealType> ( max[0], max[1] ) ) ||
00566              segments_intersect ( p1, p2, aol::Vec2<RealType> ( max[0], min[1] ),  aol::Vec2<RealType> ( max[0], max[1] ) ) );
00567   }
00568 
00569   inline static bool segments_intersect ( const aol::Vec2<RealType> &p00, const aol::Vec2<RealType> &p01,
00570                                           const aol::Vec2<RealType> &p10, const aol::Vec2<RealType> &p11 ) {
00571     aol::Vec2<RealType> alpha;
00572     return segments_intersect ( p00, p01, p10, p11, alpha );
00573   }
00574 
00575 
00576   inline static bool segments_intersect ( const aol::Vec2<RealType> &p00, const aol::Vec2<RealType> &p01,
00577                                           const aol::Vec2<RealType> &p10, const aol::Vec2<RealType> &p11,
00578                                           aol::Vec2<RealType> &Alpha ) {
00579     aol::Matrix22<RealType> mat, inv;
00580     aol::Vec2<RealType> rhs;
00581 
00582     mat[0] = p00;
00583     mat[0] -= p01;
00584 
00585     mat[1] = p11;
00586     mat[1] -= p10;
00587 
00588     mat.transpose();
00589 
00590     rhs = p11;
00591     rhs -= p01;
00592 
00593     try {
00594       mat.make_inverse ( inv );
00595     } catch ( aol::Exception &ex ) {
00596       ex.consume();
00597       aol::Vec2<RealType> v, w;
00598       v  = p01;
00599       v -= p00;
00600 
00601       w  = p10;
00602       w -= p00;
00603 
00604 
00605       RealType alpha = ( v * w ) / ( w * w );
00606 
00607       v *= alpha;
00608       v += p00;
00609 
00610       if ( alpha >= 0. && alpha <= 1. && euclidianDist ( v, p10 ) < 1e-10 ) {
00611         return true;
00612       }
00613 
00614       w = p11;
00615       w -= p00;
00616 
00617 
00618       alpha = ( v * w ) / ( w * w );
00619 
00620       v *= alpha;
00621       v += p00;
00622 
00623 
00624       if ( alpha >= 0. && alpha <= 1. && euclidianDist ( v, p11 ) < 1e-10 ) {
00625         return true;
00626       }
00627 
00628       return false;
00629     }
00630 
00631     inv.mult ( rhs, Alpha );
00632 
00633     if ( Alpha[0] >= 0. && Alpha[0] <= 1. &&
00634          Alpha[1] >= 0. && Alpha[1] <= 1. ) {
00635       return true;
00636     } else {
00637       return false;
00638     }
00639   }
00640 };
00641 
00642 
00643 
00644 template <class RealType>
00645 class Intersector<AlignedQuad<RealType>, LineSegment<RealType, qc::QC_2D> > {
00646 public:
00647 
00648   static bool has_intersection ( const AlignedQuad<RealType> &c,
00649                                  const LineSegment<RealType, qc::QC_2D> &s )  {
00650 
00651     aol::Vec2<RealType> bbmin, bbmax, cut;
00652     s.getBoundingBox ( bbmin, bbmax );
00653 
00654     bool completely_inside = true;
00655     for ( int i = 0; i < 2; i++ ) {
00656       if ( ! ( c._min[i] <= bbmin[i] &&
00657                c._max[i] >= bbmax[i] ) ) {
00658         completely_inside = false;
00659         break;
00660       }
00661     }
00662 
00663     if ( completely_inside ) {
00664       return true;
00665     }
00666 
00667     if ( bbmin[0] <= c._max[0] && bbmax[0] >= c._max[0] ) {
00668       aol::Vec2<RealType> a ( c._max[0], c._min[1] );
00669       aol::Vec2<RealType> b ( c._max[0], c._max[1] );
00670       aol::LineSegment<RealType, qc::QC_2D> qs ( a, b );
00671       if ( Intersector <
00672            aol::LineSegment<RealType, qc::QC_2D>,
00673            aol::LineSegment<RealType, qc::QC_2D> >:: cut ( qs, s, cut ) ) {
00674         return true;
00675       }
00676     }
00677 
00678     if ( bbmin[0] <= c._min[0] && bbmax[0] >= c._min[0] ) {
00679       aol::Vec2<RealType> a ( c._min[0], c._min[1] );
00680       aol::Vec2<RealType> b ( c._min[0], c._max[1] );
00681       aol::LineSegment<RealType, qc::QC_2D> qs ( a, b );
00682       if ( Intersector <
00683            aol::LineSegment<RealType, qc::QC_2D>,
00684            aol::LineSegment<RealType, qc::QC_2D> >:: cut ( qs, s, cut ) ) {
00685         return true;
00686       }
00687     }
00688 
00689 
00690     if ( bbmin[1] <= c._max[1] && bbmax[1] >= c._max[1] ) {
00691       aol::Vec2<RealType> a ( c._min[0], c._max[1] );
00692       aol::Vec2<RealType> b ( c._max[0], c._max[1] );
00693       aol::LineSegment<RealType, qc::QC_2D> qs ( a, b );
00694       if ( Intersector <
00695            aol::LineSegment<RealType, qc::QC_2D>,
00696            aol::LineSegment<RealType, qc::QC_2D> >:: cut ( qs, s, cut ) ) {
00697         return true;
00698       }
00699     }
00700 
00701     if ( bbmin[1] <= c._min[1] && bbmax[1] >= c._min[1] ) {
00702       aol::Vec2<RealType> a ( c._min[0], c._min[1] );
00703       aol::Vec2<RealType> b ( c._max[0], c._min[1] );
00704       aol::LineSegment<RealType, qc::QC_2D> qs ( a, b );
00705       if ( Intersector <
00706            aol::LineSegment<RealType, qc::QC_2D>,
00707            aol::LineSegment<RealType, qc::QC_2D> >:: cut ( qs, s, cut ) ) {
00708         return true;
00709       }
00710     }
00711     return false;
00712   }
00713 };
00714 
00715 
00716 template <class RealType>
00717 class Intersector<AlignedCube<RealType>, LineSegment<RealType, qc::QC_3D> > {
00718 public:
00719 
00720   /**
00721    * Heavily optimized code, taken from http://www.3dkingdoms.com/weekly/bbox.cpp,
00722    * CBBox::IsLineInBox( const CVec3& L1, const CVec3& L2 )
00723    *
00724    * \author Berkels
00725    */
00726   static bool has_intersection ( const AlignedCube<RealType> &Cube,
00727                                  const LineSegment<RealType, qc::QC_3D> &Segment )  {
00728     // Translate everything so that the center of the cube is the origin.
00729     const aol::Vec3<RealType> cubeExtent = (Cube._max - Cube._min) / 2.0f;
00730     const aol::Vec3<RealType> cubeMid = (Cube._min + Cube._max) * 0.5f;
00731     const aol::Vec3<RealType> translatesSegmentBegin = Segment.beginPoint() - cubeMid;
00732     const aol::Vec3<RealType> translatesSegmentEnd = Segment.endPoint() - cubeMid;
00733 
00734     // Get line midpoint and extent
00735     const aol::Vec3<RealType> sMid = (translatesSegmentBegin + translatesSegmentEnd) * 0.5f;
00736     const aol::Vec3<RealType> s = (translatesSegmentBegin - sMid);
00737     const aol::Vec3<RealType> sExt = aol::Vec3<RealType>( aol::Abs(s[0]), aol::Abs(s[1]), aol::Abs(s[2]) );
00738 
00739     // Use Separating Axis Test
00740     // Separation vector from box center to line center is sMid, since the line is in box space
00741     if ( aol::Abs( sMid[0] ) > cubeExtent[0] + sExt[0] ) return false;
00742     if ( aol::Abs( sMid[1] ) > cubeExtent[1] + sExt[1] ) return false;
00743     if ( aol::Abs( sMid[2] ) > cubeExtent[2] + sExt[2] ) return false;
00744     // Crossproducts of line and each axis
00745     if ( aol::Abs( sMid[1] * s[2] - sMid[2] * s[1])  >  (cubeExtent[1] * sExt[2] + cubeExtent[2] * sExt[1]) ) return false;
00746     if ( aol::Abs( sMid[0] * s[2] - sMid[2] * s[0])  >  (cubeExtent[0] * sExt[2] + cubeExtent[2] * sExt[0]) ) return false;
00747     if ( aol::Abs( sMid[0] * s[1] - sMid[1] * s[0])  >  (cubeExtent[0] * sExt[1] + cubeExtent[1] * sExt[0]) ) return false;
00748     // No separating axis, the line intersects
00749     return true;
00750   }
00751 
00752   /**
00753    * Interprets the element as cube given by (Cube[0], Cube[1], Cube[2]) and (Cube[0] + 1, Cube[1] + 1, Cube[2] + 1)
00754    *
00755    * \author Berkels
00756    */
00757   static bool has_intersection ( const qc::Element &Cube,
00758                                  const LineSegment<RealType, qc::QC_3D> &Segment )  {
00759     aol::AlignedCube<RealType> cube ( Cube[0], Cube[1], Cube[2],
00760                                       Cube[0] + 1, Cube[1] + 1, Cube[2] + 1 );
00761     return has_intersection ( cube, Segment );
00762   }
00763 
00764   static bool inline GetIntersection( RealType fDst1, RealType fDst2, aol::Vec3<RealType> P1, aol::Vec3<RealType> P2, aol::Vec3<RealType> &Hit) {
00765     if ( (fDst1 * fDst2) >= 0.0f) return 0;
00766     if ( fDst1 == fDst2) return 0;
00767     Hit = P1 + (P2-P1) * ( -fDst1/(fDst2-fDst1) );
00768     return 1;
00769   }
00770 
00771   static bool inline InBox( aol::Vec3<RealType> Hit, aol::Vec3<RealType> B1, aol::Vec3<RealType> B2, const int Axis) {
00772     if ( Axis==1 && Hit[2] > B1[2] && Hit[2] < B2[2] && Hit[1] > B1[1] && Hit[1] < B2[1]) return 1;
00773     if ( Axis==2 && Hit[2] > B1[2] && Hit[2] < B2[2] && Hit[0] > B1[0] && Hit[0] < B2[0]) return 1;
00774     if ( Axis==3 && Hit[0] > B1[0] && Hit[0] < B2[0] && Hit[1] > B1[1] && Hit[1] < B2[1]) return 1;
00775     return 0;
00776   }
00777 
00778   /**
00779    * Returns true if line (L1, L2) intersects with the box (B1, B2).
00780    * If the line intersects with the box, an intersection point is returned
00781    * in Hit. In case the line is completely inside the box, Hit is set to L1,
00782    * otherwise it is a point on the boundary of the box.
00783    *
00784    * Code from http://www.3dkingdoms.com/weekly/weekly.php?a=3.
00785    *
00786    * \author Berkels
00787    *
00788    * \note Should only be used if one actually needs the intersection point as
00789    *       the version of has_intersection that doesn't calculate the point is
00790    *       considerably faster.
00791    */
00792   static bool has_intersection ( const AlignedCube<RealType> &Cube,
00793                                  const LineSegment<RealType, qc::QC_3D> &Segment,
00794                                  aol::Vec3<RealType> &Hit )  {
00795     const aol::Vec3<RealType> &B1 = Cube._min;
00796     const aol::Vec3<RealType> &B2 = Cube._max;
00797     const aol::Vec3<RealType> &L1 = Segment.beginPoint();
00798     const aol::Vec3<RealType> &L2 = Segment.endPoint();
00799 
00800     for ( int i = 0; i < 3; ++ i ) {
00801       if (L2[i] < B1[i] && L1[i] < B1[i]) return false;
00802       if (L2[i] > B2[i] && L1[i] > B2[i]) return false;
00803     }
00804 
00805     // Is the line completely inside the box?
00806     if ( L1[0] > B1[0] && L1[0] < B2[0] &&
00807          L1[1] > B1[1] && L1[1] < B2[1] &&
00808          L1[2] > B1[2] && L1[2] < B2[2]) {
00809       Hit = L1;
00810       return true;
00811     }
00812 
00813     if ( (GetIntersection( L1[0]-B1[0], L2[0]-B1[0], L1, L2, Hit) && InBox( Hit, B1, B2, 1 ))
00814       || (GetIntersection( L1[1]-B1[1], L2[1]-B1[1], L1, L2, Hit) && InBox( Hit, B1, B2, 2 ))
00815       || (GetIntersection( L1[2]-B1[2], L2[2]-B1[2], L1, L2, Hit) && InBox( Hit, B1, B2, 3 ))
00816       || (GetIntersection( L1[0]-B2[0], L2[0]-B2[0], L1, L2, Hit) && InBox( Hit, B1, B2, 1 ))
00817       || (GetIntersection( L1[1]-B2[1], L2[1]-B2[1], L1, L2, Hit) && InBox( Hit, B1, B2, 2 ))
00818       || (GetIntersection( L1[2]-B2[2], L2[2]-B2[2], L1, L2, Hit) && InBox( Hit, B1, B2, 3 )))
00819       return true;
00820 
00821     return false;
00822   }
00823 
00824 };
00825 
00826 
00827 //! Conversion from Cartesian to homogeneous coordinates (copy entries and write 1 to last component)
00828 template< typename RealType, int Dim >
00829 inline void CartesianToHomogeneousCoordinates ( const aol::Vec<Dim,RealType> &arg, aol::Vec<Dim+1,RealType> &dest ) {
00830   for ( int d = 0; d < Dim; ++d ) {
00831     dest[d] = arg[d];
00832   }
00833   dest[Dim] = aol::NumberTrait<RealType>::one;
00834 }
00835 
00836 //! Conversion from homogeneous to Cartesian coordinates (division by last component)
00837 template< typename RealType, int Dim >
00838 inline void HomogeneousToCartesianCoordinates ( const aol::Vec<Dim+1,RealType> &arg, aol::Vec<Dim,RealType> &dest ) {
00839   for ( int d = 0; d < Dim; ++d ) {
00840     dest[d] = arg[d] / arg[Dim];
00841   }
00842 }
00843 
00844 //! perform a transformation given for homoegeneous coordinates to vectors in Cartesian coordinates
00845 template< typename RealType, int Dim >
00846 void TransformCartesianCoordinatesByHomogeneousMapping ( const aol::Vec<Dim, RealType> &arg, const aol::Mat<Dim+1, Dim+1, RealType> &transMat, aol::Vec<Dim, RealType> &dest ) {
00847   aol::Vec<Dim+1,RealType> argH, destH;
00848   aol::CartesianToHomogeneousCoordinates ( arg, argH );
00849   transMat.mult ( argH, destH );
00850   aol::HomogeneousToCartesianCoordinates ( destH, dest );
00851 }
00852 
00853 } // end namespace aol
00854 
00855 #endif

Generated on Fri Sep 9 2011 21:07:48 for QuocMesh by doxygen 1.7.1