Jafar: modules/geom/include/geom/DistanceImpl.hpp Source File

00001 /* $Id$ */
00002 
00003 #include <kernel/jafarMacro.hpp>
00004 
00005 #include "Segment.hpp"
00006 #include "Intersection.hpp"
00007 
00008 namespace jafar {
00009   namespace geom {
00010     template<int dimension>
00011     inline double distance(const typename Atom<dimension>::HomogenousVecD& v1, const typename Atom<dimension>::HomogenousVecD& v2)
00012     {
00013       JFR_ASSERT( fabs( v1(dimension) - 1.0 ) < 1e-6 , "Homogenous coefficient need to be 1" );
00014       JFR_ASSERT( fabs( v2(dimension) - 1.0 ) < 1e-6 , "Homogenous coefficient need to be 1" );
00015       return ublas::norm_2( v1 - v2 );
00016     }
00017     template<int dimension>
00018     inline double distance(const Point<dimension>& pt, const typename Atom<dimension>::HomogenousVecD& v)
00019     {
00020       return distance<dimension>(pt.homogenousCoordinates(), v);
00021     }
00022     template<int dimension>
00023     inline double distance(const Point<dimension>& pt1, const Point<dimension>& pt2)
00024     {
00025       return distance<dimension>( pt1.homogenousCoordinates(), pt2.reference()->convertIn( pt2.homogenousCoordinates(), pt1.reference() ) );
00026     }
00027     template<int dimension>
00028     inline double distance(const Point<dimension>& pt, const Line<dimension>& line)
00029     {
00030       return distance( line, pt );
00031     }
00032     template<int dimension>
00033     inline double distance(const Point<dimension>& point, const HyperPlane<dimension>& plan)
00034     {
00035       return distance(plan, point);
00036     }
00037     template<int dimension>
00038     inline double distance(const Point<dimension>& p, const Segment<dimension>& seg)
00039     {
00040       return distance(seg, p);
00041     }
00042     // Line
00043     template<int dimension>
00044     inline double distance(const Line<dimension>& line, const typename Atom<dimension>::HomogenousVecD& v)
00045     {
00046       return ublas::norm_2( v - line.project(v) );
00047     }
00048     template<int dimension>
00049     inline double distance(const Line<dimension>& line, const Point<dimension>& point)
00050     {
00051       return distance(line, point.reference()->convertIn( point.homogenousCoordinates(), line.reference() ) );
00052     }
00053     template<int dimension>
00054     inline double distance(const Line<dimension>& line1, const Line<dimension>& line2)
00055     {
00056       typename Atom<dimension>::HomogenousMatrixD m;
00057       line2.reference()->localToRepere(m, line1.reference());
00058       typename Atom<dimension>::HomogenousVecD o1 = line1.origin();
00059       typename Atom<dimension>::HomogenousVecD u1 = line1.direction();
00060       typename Atom<dimension>::HomogenousVecD u2 = ublas::prod(m, line2.direction());
00061       typename Atom<dimension>::HomogenousVecD o2 = ublas::prod(m, line2.origin());
00062       typename Atom<dimension>::HomogenousVecD O1O2 = o1 - o2;
00063       double u1u2 = ublas::inner_prod(u1, u2);
00064       double denom = 1.0 - u1u2 * u1u2;
00065       if(denom > 1e-6)
00066       {
00067         double u1O1O2 = ublas::inner_prod(u1, O1O2);
00068         double u2O1O2 = ublas::inner_prod(u2, O1O2);
00069         double t2 = ( u2O1O2 - u1u2 * u1O1O2) / denom;
00070         double t1 = ( u1u2 * u2O1O2 - u1O1O2) / denom;
00071         return ublas::norm_2( o2 + t2 * u2 - o1 - t1 * u1 );
00072       } else { // Parallel
00073         return distance( line1, o2 );
00074       }
00075     }
00076     template<int dimension>
00077     inline double distance(const Line<dimension>& line, const HyperPlane<dimension>& plan)
00078     {
00079       typename Atom<dimension>::HomogenousMatrixD m;
00080       line.reference()->localToRepere(m, plan.reference());
00081       if( fabs( ublas::inner_prod( ublas::prod(m, line.direction()), plan.normal() ) < 10e-6 ) )
00082       {
00083         return distance( plan, ublas::prod(m, line.origin()) );
00084       } else {
00085         return 0.0;
00086       }
00087     }
00088     template<int dimension>
00089     inline double distance(const HyperPlane<dimension>& plan, const typename Atom<dimension>::HomogenousVecD& v)
00090     {
00091       return fabs( ublas::inner_prod(v, plan.equation() ) );
00092     }
00093     template<int dimension>
00094     inline double distance(const HyperPlane<dimension>& plan, const Point<dimension>& point)
00095     {
00096       return distance(plan, point.reference()->convertIn( point.homogenousCoordinates(), plan.reference() ) );
00097     }
00098     template<int dimension>
00099     inline double distance(const HyperPlane<dimension>& plan, const Line<dimension>& line)
00100     {
00101       return distance(line, plan);
00102     }
00103     template<int dimension>
00104     inline double distance(const HyperPlane<dimension>& plan1, const HyperPlane<dimension>& plan2)
00105     {
00106       typename Atom<dimension>::HomogenousMatrixD m;
00107       plan2.reference()->localToRepere(m, plan1.reference());
00108       if( fabs( ublas::inner_prod(plan1.normal(), ublas::prod( m, plan2.normal()) ) ) < 10e-6 )
00109       {
00110         return distance(plan1, ublas::prod(m, plan2.origin()));
00111       } else {
00112         return 0.0;
00113       }
00114     }
00115     template<int dimension>
00116     inline double distance(const Segment<dimension>& seg, 
00117                            const typename Atom<dimension>::HomogenousVecD& p_coor )
00118     {
00119       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00120       HomogenousVecD firstPoint = seg.firstPoint();
00121       HomogenousVecD lastPoint = seg.lastPoint();
00122       HomogenousVecD dir = lastPoint - firstPoint;
00123       
00124       // Compute the projection of the point
00125       HomogenousVecD proj_p_coor = seg.project(p_coor);
00126       
00127       // Check the position of the projection to determine wether to use extremeties or support
00128       double nd = ublas::norm_2(dir);
00129       double alpha = ublas::inner_prod( dir, proj_p_coor - firstPoint ) / (nd * nd);
00130       if( alpha < 0.0 ) return ublas::norm_2( firstPoint - p_coor );
00131       else if( alpha > 1.0 ) return ublas::norm_2( lastPoint - p_coor );
00132       else return ublas::norm_2( proj_p_coor - p_coor );
00133     }
00134     template<int dimension>
00135     inline double distance(const Segment<dimension>& seg, const HyperPlane<dimension>& hp)
00136     {
00137       JFR_ASSERT(seg.reference() == hp.reference(), "Unimplemented different references");
00138       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00139       HomogenousVecD intersectPoint;
00140       bool v = intersection<dimension>(seg, hp, intersectPoint);
00141       if(v)
00142       {
00143         HomogenousVecD firstPoint = seg.firstPoint();
00144         HomogenousVecD lastPoint = seg.lastPoint();
00145         HomogenousVecD dir = lastPoint - firstPoint;
00146         double nd = ublas::norm_2(dir);
00147         double alpha = ublas::inner_prod( dir, intersectPoint - firstPoint ) / (nd * nd);
00148         if( alpha < 0.0 ) return distance( hp, firstPoint );
00149         else if( alpha > 1.0 ) return distance( hp, lastPoint );
00150         else return 0.0;
00151       } else {
00152         return distance(hp, seg.firstPoint() );
00153       }
00154     }
00155     template<int dimension>
00156     inline double distance(const Segment<dimension>& seg, const Line<dimension>& l)
00157     {
00158       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00159       HomogenousVecD v1, v2;
00160       seg.closestPoint(l, v1, v2);
00161       HomogenousVecD a = seg.lastPoint() - seg.firstPoint();
00162       double t = ublas::inner_prod(v1 - seg.firstPoint(), a) / ublas::inner_prod(a, a);
00163       if(t < 0)
00164       {
00165         return distance(l, seg.firstPoint());
00166       } else if( t > 1.0 )
00167       {
00168         return distance(l, seg.lastPoint());
00169       } else {
00170         return ublas::norm_2(v2 - v1);
00171       }
00172     }
00173     template<int dimension>
00174     inline double distance(const Segment<dimension>& seg, const Point<dimension>& p)
00175     {
00176       return distance(seg, p.reference()->convertIn( p.homogenousCoordinates(), seg.reference()));
00177     }
00178     template<int dimension>
00179     inline double distance(const Segment<dimension>& s, const PolyLine<dimension>& pl)
00180     {
00181       return distancePolyLine(pl, s);
00182     }
00183     template<int dimension>
00184     inline double distance(const Line<dimension>& l, const Segment<dimension>& seg)
00185     {
00186       return distance(seg, l);
00187     }
00188     template<int dimension>
00189     inline double distance(const HyperPlane<dimension>& hp, const Segment<dimension>& seg)
00190     {
00191       return distance(seg, hp);
00192     }
00193     template<int dimension>
00194     inline double distance(const Segment<dimension>& seg1, const Segment<dimension>& seg2)
00195     {
00196       // see http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm
00197       typedef typename Atom<dimension>::HomogenousVecD HomogenousVecD;
00198       typename Atom<dimension>::HomogenousMatrixD m;
00199       seg2.reference()->localToRepere(m, seg1.reference());
00200       HomogenousVecD seg2P0 = ublas::prod(m, seg2.firstPoint() );
00201       HomogenousVecD seg2P1 = ublas::prod(m, seg2.lastPoint() );
00202       HomogenousVecD u = seg1.lastPoint() - seg1.firstPoint();
00203       HomogenousVecD v = seg2P1 - seg2P0;
00204       HomogenousVecD w0 = seg1.firstPoint() - seg2P0;
00205       double u2 = ublas::inner_prod(u, u);
00206       double uv = ublas::inner_prod(u, v);
00207       double v2 = ublas::inner_prod(v, v);
00208       double uw = ublas::inner_prod(u, w0);
00209       double vw = ublas::inner_prod(v, w0);
00210       double D = u2 * v2 - uv * uv;
00211       double sc, sN, sD = D;
00212       double tc, tN, tD = D;
00213       // compute the line parameters of the two closet points
00214       if( D < 1e-6 ) // the lines are almost parallel
00215       {
00216         // force using P0 on segment S1 to prevent a division by 0.0
00217         sN = 0.0;
00218         sD = 1.0;
00219         tN = vw;
00220         tD = v2;
00221       }
00222       else
00223       {
00224         sN = (uv * vw - v2 * uw);
00225         tN = (u2 * vw - uv * uw);
00226         if(sN < 0.0) // the s=0 edge is visible
00227         {
00228           sN = 0.0;
00229           tN = vw;
00230           tD = v2;
00231         }
00232         else if(sN > sD) // the s=1 edge is visible
00233         {
00234           sN = sD;
00235           tN = vw + uv;
00236           tD = v2;
00237         }
00238       }
00239       if(tN < 0.0) // the t = 0 edge is visible
00240       {
00241         tN = 0.0;
00242         // Recompute sN for this edge
00243         if( uw > 0.0 )
00244         {
00245           sN = 0.0;
00246         }
00247         else if( -uw > u2 )
00248         {
00249           sN = sD;
00250         }
00251         else
00252         {
00253           sN = -uw;
00254           sD = u2;
00255         }
00256       }
00257       else if(tN > tD) // the t=1 edge is visible
00258       {
00259         tN = tD;
00260         // recompute sc for this edge
00261         if( (-uw+uv) < 0.0)
00262         {
00263           sN = 0;
00264         }
00265         else if( (-uw + uv) > u2 )
00266         {
00267           sN = sD;
00268         }
00269         else
00270         {
00271           sN = -uw + uv;
00272           sD = u2;
00273         }
00274       }
00275       sc = (fabs(sN) < 1e-6 ? 0.0 : sN / sD);
00276       tc = (fabs(tN) < 1e-6 ? 0.0 : tN / tD);
00277       HomogenousVecD dP = w0 + sc * u - tc * v;
00278       return ublas::norm_2(dP);
00279     }
00280     // PolyLine
00281     template<int dimension, typename _T2_>
00282     inline double distancePolyLine(const PolyLine<dimension>& pl, const _T2_& t2)
00283     {
00284       std::vector< Segment<dimension> > segments = pl.segments();
00285       typename std::vector< Segment<dimension> >::iterator it = segments.begin();
00286       double minDistance = distance( *it, t2 );
00287       ++it;
00288       for( ; it != segments.end(); ++it)
00289       {
00290         double d = distance( *it, t2 );
00291         if( d < minDistance) minDistance = d;
00292       }
00293       return minDistance;
00294     }
00295     template<int dimension, typename _T2_>
00296     inline double distance(const PolyLine<dimension>& pl, const _T2_& t2)
00297     {
00298       return distancePolyLine(pl, t2);
00299     }
00300     template<int dimension, typename _T1_>
00301     inline double distance(const _T1_& t1, const PolyLine<dimension>& pl)
00302     {
00303       return distancePolyLine(pl, t1);
00304     }
00305     template<int dimension>
00306     inline double distance(const PolyLine<dimension>& p, const Facet<dimension>& f )
00307     {
00308       return distance(f.spacePolyLine(), p);
00309     }
00310     template<int dimension>
00311     inline double distance(const PolyLine<dimension>& pl1, const PolyLine<dimension>& pl2)
00312     {
00313       return distancePolyLine(pl1, pl2);
00314     }
00315     template<int dimension>
00316     inline double distance(const PolyLine<dimension>& pl, const Segment<dimension>& s)
00317     {
00318       return distancePolyLine(pl, s);
00319     }
00320     template<int dimension>
00321     inline double distance(const Repere<dimension>& r, const PolyLine<dimension>& pl)
00322     {
00323       return distancePolyLine(pl, r);
00324     }
00325     template<int dimension>
00326     inline double distance(const PolyLine<dimension>& pl, const Repere<dimension>& r)
00327     {
00328       return distancePolyLine(pl, r);
00329     }
00330     // Facet
00331     template<int dimension, typename _T2_>
00332     inline double distance(const Facet<dimension>& f, const _T2_& t2)
00333     {
00334       return distance(f.spacePolyLine(), t2);
00335     }
00336     template<int dimension, typename _T1_>
00337     inline double distance(const _T1_& t1, const Facet<dimension>& f)
00338     {
00339       return distance(f.spacePolyLine(), t1);
00340     }
00341     template<int dimension>
00342     inline double distance(const Facet<dimension>& f1, const Facet<dimension>& f2)
00343     {
00344       return distance(f1.spacePolyLine(), f2.spacePolyLine());
00345     }
00346     template<int dimension>
00347     inline double distance(const Facet<dimension>& f, const Segment<dimension>& s)
00348     {
00349       return distance(f.spacePolyLine(), s);
00350     }
00351     template<int dimension>
00352     inline double distance(const Facet<dimension>& f, const PolyLine<dimension>& p)
00353     {
00354       return distance(f.spacePolyLine(), p);
00355     }
00356     template<int dimension>
00357     inline double distance(const Repere<dimension>& r, const Facet<dimension>& f)
00358     {
00359       return distance(f.spacePolyLine(), r);
00360     }
00361     template<int dimension>
00362     inline double distance(const Facet<dimension>& f, const Repere<dimension>& r)
00363     {
00364       return distance(f.spacePolyLine(), r);
00365     }
00366     // Repere
00367     template<int dimension, typename _T2_>
00368     inline double distance(const Repere<dimension>& r, const _T2_& t2)
00369     {
00370       typename Atom<dimension>::HomogenousVecD v = jblas::zero_vec(dimension+1);
00371       v(dimension) = 1.0;
00372       return distance( t2, r.convertIn( v , t2.reference() ) );
00373     }
00374     template<int dimension, typename _T1_>
00375     inline double distance(const _T1_& t1, const Repere<dimension>& r)
00376     {
00377       typename Atom<dimension>::HomogenousVecD v = jblas::zero_vec(dimension+1);
00378       v(dimension) = 1.0;
00379       return distance( t1, r.convertIn( v , t1.reference() ) );
00380     }
00381     template<int dimension>
00382     inline double distance(const Repere<dimension>& r1, const Repere<dimension>& r2)
00383     {
00384       typename Atom<dimension>::HomogenousVecD v = jblas::zero_vec(dimension+1);
00385       v(dimension) = 1.0;
00386       return ublas::norm_2( r1.origin() - r2.convertIn( v , &r1) );
00387     }
00388   }
00389 }