shape_reconstruction: intersection2.h Source File

Go to the documentation of this file.
00001 /****************************************************************************
00002 * VCGLib                                                            o o     *
00003 * Visual and Computer Graphics Library                            o     o   *
00004 *                                                                _   O  _   *
00005 * Copyright(C) 2004                                                \/)\/    *
00006 * Visual Computing Lab                                            /\/|      *
00007 * ISTI - Italian National Research Council                           |      *
00008 *                                                                    \      *
00009 * All rights reserved.                                                      *
00010 *                                                                           *
00011 * This program is free software; you can redistribute it and/or modify      *   
00012 * it under the terms of the GNU General Public License as published by      *
00013 * the Free Software Foundation; either version 2 of the License, or         *
00014 * (at your option) any later version.                                       *
00015 *                                                                           *
00016 * This program is distributed in the hope that it will be useful,           *
00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of            *
00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
00019 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
00020 * for more details.                                                         *
00021 *                                                                           *
00022 ****************************************************************************/
00023 /****************************************************************************
00024 History
00025 
00026 $Log: not supported by cvs2svn $
00027 Revision 1.6  2007/05/08 12:11:58  pietroni
00028 added circle-line intersection
00029 
00030 
00031 ****************************************************************************/
00032 
00033 
00034 
00035 #ifndef __VCGLIB_INTERSECTION_2
00036 #define __VCGLIB_INTERSECTION_2
00037 #include <vcg/space/line2.h>
00038 #include <vcg/space/ray2.h>
00039 #include <vcg/space/segment2.h>
00040 #include <vcg/space/point2.h>
00041 #include <vcg/space/triangle2.h>
00042 #include <vcg/space/box2.h>
00043 #include <vector>
00044 
00045 
00046 
00047 namespace vcg {
00055 
00056         template<class SCALAR_TYPE>
00057         inline bool Convex(const Point2<SCALAR_TYPE> & p0,const Point2<SCALAR_TYPE> & p1,const Point2<SCALAR_TYPE> & p2)
00058         {
00059                 const SCALAR_TYPE EPS= SCALAR_TYPE(1e-8);
00060                 return (((p0-p1)^(p2-p1))<=EPS);
00061         }
00062 
00065         template<class SCALAR_TYPE>
00066         inline bool LineLineIntersection(const vcg::Line2<SCALAR_TYPE> & l0,
00067                 const vcg::Line2<SCALAR_TYPE> & l1,
00068                 Point2<SCALAR_TYPE> &p)
00069         {
00070                 const SCALAR_TYPE Eps= SCALAR_TYPE(1e-8);
00072                 SCALAR_TYPE x1=l0.Origin().X();
00073                 SCALAR_TYPE y1=l0.Origin().Y(); 
00074                 SCALAR_TYPE x2=x1+l0.Direction().X();
00075                 SCALAR_TYPE y2=y1+l0.Direction().Y(); 
00076 
00078                 SCALAR_TYPE x3=l1.Origin().X();
00079                 SCALAR_TYPE y3=l1.Origin().Y(); 
00080                 SCALAR_TYPE x4=x3+l1.Direction().X();
00081                 SCALAR_TYPE y4=y3+l1.Direction().Y(); 
00082 
00084 
00086                 SCALAR_TYPE den=((x1-x2)*(y3-y4))-((y1-y2)*(x3-x4));
00087                 if (fabs(den)<Eps)
00088                         return false;
00089 
00090                 SCALAR_TYPE d0=(x1*y2)-(y1*x2);
00091                 SCALAR_TYPE d1=(x3*y4)-(y3*x4);
00092                 SCALAR_TYPE numx=(d0*(x3-x4))-(d1*(x1-x2));
00093                 SCALAR_TYPE numy=(d0*(y3-y4))-(d1*(y1-y2));
00094 
00095                 p.X()=numx/den;
00096                 p.Y()=numy/den;
00097                 return true;
00098         }
00099 
00102         template<class SCALAR_TYPE>
00103         inline bool RayLineIntersection(const vcg::Line2<SCALAR_TYPE> & l,
00104                 const vcg::Ray2<SCALAR_TYPE> & r,
00105                 Point2<SCALAR_TYPE> &p)
00106         {
00108                 vcg::Line2<SCALAR_TYPE> l_test;
00109                 l_test.Set(r.Origin(),r.Direction());
00110                 if (!LineLineIntersection(l,l_test,p))
00111                         return false;
00112                 Point2<SCALAR_TYPE> dir=p-r.Origin();
00113                 dir.Normalize();
00114                 return (dir*r.Direction()>0);
00115         }
00116 
00117 
00119         template<class SCALAR_TYPE>
00120         inline bool RaySegmentIntersection(const vcg::Ray2<SCALAR_TYPE> & r,
00121                 const vcg::Segment2<SCALAR_TYPE> &seg,
00122                 Point2<SCALAR_TYPE> &p_inters)
00123         {
00125                 vcg::Line2<SCALAR_TYPE> line2;
00126                 line2.SetOrigin(seg.P0());
00127                 vcg::Point2<SCALAR_TYPE> dir=seg.P1()-seg.P0();
00128                 dir.Normalize();
00129                 line2.SetDirection(dir);
00130                 if(!RayLineIntersection<SCALAR_TYPE>(line2,r,p_inters))
00131                         return false;
00134                 SCALAR_TYPE d0=(seg.P1()-p_inters).Norm();
00135                 SCALAR_TYPE d1=(seg.P0()-p_inters).Norm();
00136                 SCALAR_TYPE length=(seg.P0()-seg.P1()).Norm();
00137                 return ((d0<length)&&(d1<length));
00138         }
00139 
00141     template<class SCALAR_TYPE>
00142     inline bool RayBoxIntersection(const vcg::Ray2<SCALAR_TYPE> & r,
00143                                    const vcg::Box2<SCALAR_TYPE> &bbox,
00144                                     Point2<SCALAR_TYPE> &p_inters)
00145     {
00147         vcg::Segment2<SCALAR_TYPE> S[4];
00148         for (int i=0;i<4;i++)
00149             S[i]=vcg::Segment2<SCALAR_TYPE>(bbox.P(i),bbox.P((i+1)%4));
00150 
00151         SCALAR_TYPE mind=std::numeric_limits<SCALAR_TYPE>::max();
00152         bool found=false;
00153         for (int i=0;i<4;i++)
00154         {
00155              Point2<SCALAR_TYPE> p_inters_test;
00156             if (!RaySegmentIntersection(r,S[i],p_inters_test))continue;
00157             SCALAR_TYPE Norm=(p_inters_test-r.Origin()).Norm();
00158             if (Norm<mind)
00159             {
00160                 mind=Norm;
00161                 p_inters=p_inters_test;
00162                 found=true;
00163             }
00164         }
00165         return found;
00166     }
00167 
00169         template<class SCALAR_TYPE>
00170         inline bool LineSegmentIntersection(const vcg::Line2<SCALAR_TYPE> & line,
00171                 const vcg::Segment2<SCALAR_TYPE> &seg,
00172                 Point2<SCALAR_TYPE> &p_inters)
00173         {
00175                 vcg::Line2<SCALAR_TYPE> line2;
00176                 line2.SetOrigin(seg.P0());
00177                 vcg::Point2<SCALAR_TYPE> dir=seg.P1()-seg.P0();
00178                 dir.Normalize();
00179                 line2.SetDirection(dir);
00180                 if(!LineLineIntersection(line,line2,p_inters))
00181                         return false;
00184                 SCALAR_TYPE d0=(seg.P1()-p_inters).Norm();
00185                 SCALAR_TYPE d1=(seg.P0()-p_inters).Norm();
00186                 SCALAR_TYPE length=(seg.P0()-seg.P1()).Norm();
00187                 return ((d0<length)&&(d1<length));
00188         }
00189 
00191         template<class SCALAR_TYPE>
00192         inline bool SegmentSegmentIntersection(const vcg::Segment2<SCALAR_TYPE> &seg0,
00193                 const vcg::Segment2<SCALAR_TYPE> &seg1,
00194                 Point2<SCALAR_TYPE> &p_inters)
00195         {
00196                 const SCALAR_TYPE Eps= SCALAR_TYPE(1e-8);
00197                 SCALAR_TYPE lambda0,lambda1;
00198         const Point2<SCALAR_TYPE> & p0 = seg0.P0();
00199         const Point2<SCALAR_TYPE> & p1 = seg0.P1();
00200         const Point2<SCALAR_TYPE> & p2 = seg1.P0();
00201         const Point2<SCALAR_TYPE> & p3 = seg1.P1();
00202 
00203                 SCALAR_TYPE a = (p1-p0)[0];
00204                 SCALAR_TYPE b = (p2-p3)[0];
00205                 SCALAR_TYPE c = (p1-p0)[1];
00206                 SCALAR_TYPE d = (p2-p3)[1];
00207 
00208                 SCALAR_TYPE e = (p2-p0)[0];
00209         SCALAR_TYPE f = (p2-p0)[1];
00210 
00211                 SCALAR_TYPE det = a*d-b*c;
00212 
00213                 lambda0 = (d*e-b*f)/det;
00214                 lambda1 = (-c*e+a*f)/det;
00215         if (fabs(det)<Eps)
00216                         return false;// they are parallell
00217                 
00218                 if (!(lambda0 >= 0.0 && lambda0 <= 1.0 && lambda1 >= 0.0 && lambda1 <= 1.0))
00219                         return false;
00220         p_inters = p0*(1-lambda0)+p1*lambda0;
00221                 return true;
00222         }
00224         template<class SCALAR_TYPE>
00225         inline bool IsInsideTrianglePoint( const Triangle2<SCALAR_TYPE> & t,const Point2<SCALAR_TYPE> & p)
00226         {
00227                 Point2<SCALAR_TYPE> p0=t.P0(0);
00228                 Point2<SCALAR_TYPE> p1=t.P0(1);
00229                 Point2<SCALAR_TYPE> p2=t.P0(2);
00230 
00232                 vcg::Box2<SCALAR_TYPE> b2d;
00233                 b2d.Add(p0);
00234                 b2d.Add(p1);
00235                 b2d.Add(p2);
00236                 if (!b2d.IsIn(p))
00237                         return false;
00238 
00240                 if (!Convex(p0,p1,p2))
00241                         std::swap<Point2<SCALAR_TYPE> >(p1,p2);
00242                 return((Convex(p,p0,p1))&&(Convex(p,p1,p2))&&(Convex(p,p2,p0)));
00243                 //return((Convex(p,p0,p1))&&(Convex(p,p1,p2))&&(Convex(p,p2,p0)));
00244         }
00245 
00246         template<class ScalarType>
00247         bool TriangleTriangleIntersect2D(const vcg::Triangle2<ScalarType> &tr0,
00248                 const vcg::Triangle2<ScalarType> &tr1)
00249         {
00251                 vcg::Box2<ScalarType> bbtr0;
00252                 bbtr0.Add(tr0.P(0));
00253                 bbtr0.Add(tr0.P(1));
00254                 bbtr0.Add(tr0.P(2));
00255                 vcg::Box2<ScalarType> bbtr1;
00256                 bbtr1.Add(tr1.P(0));
00257                 bbtr1.Add(tr1.P(1));
00258                 bbtr1.Add(tr1.P(2));
00259                 if (!bbtr0.Collide(bbtr1)) return false;
00261                 for (int i=0;i<3;i++)
00262                 {
00263                         bool inside0=vcg::IsInsideTrianglePoint(tr0,tr1.P(i));
00264                         bool inside1=vcg::IsInsideTrianglePoint(tr1,tr0.P(i));
00265                         if (inside0 || inside1) return true;
00266                 }
00269                 for (int i=0;i<3;i++)
00270                 {
00271                         for (int j=0;j<3;j++)
00272                         {
00273                                 if (i>j) continue;
00274                                 vcg::Segment2<ScalarType> seg0=vcg::Segment2<ScalarType>(tr0.P(i),tr0.P((i+1)%3));
00275                                 vcg::Segment2<ScalarType> seg1=vcg::Segment2<ScalarType>(tr1.P(j),tr1.P((j+1)%3));
00276                                 vcg::Point2<ScalarType> p_inters;
00277                                 bool intersect=SegmentSegmentIntersection(seg0,seg1,p_inters);
00278                                 if (intersect) return true;
00279                         }
00280                 }
00281                 return false;
00282         }
00283                 
00284         template <class ScalarType>
00285         bool PointInsidePolygon(vcg::Point2<ScalarType> p,
00286                                                         const std::vector<vcg::Segment2<ScalarType> > &polygon)
00287         {
00288                 int n=polygon.size();
00289                 vcg::Box2<ScalarType> BB;
00290                 for (int i=0;i<n;i++)
00291                 {
00292                         BB.Add(polygon[i].P0());
00293                         BB.Add(polygon[i].P1());
00294                 }
00295                 if (!BB.IsIn(p))return false;
00296                 ScalarType size=BB.Diag();
00298                 int inside_test=0;
00299                 for (int dir=0;dir<4;dir++)
00300                 {
00301                         int intersection=0;
00302                         vcg::Ray2<ScalarType> r;
00303                         vcg::Point2<ScalarType> direct=vcg::Point2<ScalarType>(0,0);
00304                         switch (dir) 
00305                         {
00306                                 case 0 : direct.X()=1;break;
00307                                 case 1 : direct.Y()=1;break;
00308                                 case 2 : direct.X()=-1; break;
00309                                 default :direct.Y()=-1;
00310                         }                       
00311                         r.SetOrigin(p);
00312                         r.SetDirection(direct);
00313                         for (int i=0;i<n;i++) 
00314                         {
00315                                 Point2<ScalarType> p_inters;
00316                                 if (vcg::RaySegmentIntersection(r,polygon[i],p_inters))intersection++;
00317                         }
00318                         if ((intersection%2)==1)
00319                                 inside_test++;
00320                 }
00321                 return(inside_test>2);
00322         }
00323 
00324         //intersection between a circle and a line
00325         template<class ScalarType>
00326         inline bool CircleLineIntersection(const vcg::Line2<ScalarType> & line,
00327                 const vcg::Point2<ScalarType> &center,
00328                 const ScalarType &radius,
00329                 vcg::Point2<ScalarType> &p0,
00330                 vcg::Point2<ScalarType> &p1)
00331         {
00333                 ScalarType x1,x2,y1,y2;
00334                 x1=line.Origin().X()-center.X();
00335                 y1=line.Origin().Y()-center.Y();
00336                 x2=x1+line.Direction().X();
00337                 y2=y1+line.Direction().Y();
00338 
00339                 ScalarType dx,dy,dr,D,delta,sign;
00340                 dx=x2-x1;
00341                 dy=y2-y1;
00342                 dr=sqrt(dx*dx+dy*dy);
00343                 D=x1*y2-x2*y1;
00344                 delta=radius*radius*dr*dr-D*D;
00345                 if (dy>=0)
00346                         sign=1;
00347                 else
00348                         sign=-1;
00349 
00350                 if (delta<0.000001)
00351                         return false;
00352                 else
00353                 {
00354                         p0.X()=(D*dy+sign*dx*sqrt(delta))/dr*dr;
00355                         p0.Y()=(-D*dx+fabs(dy)*sqrt(delta))/dr*dr;
00356                         p1.X()=(D*dy-sign*dx*sqrt(delta))/dr*dr;
00357                         p1.Y()=(-D*dx-fabs(dy)*sqrt(delta))/dr*dr;
00358                         p0+=center;
00359                         p1+=center;
00360                         return true;
00361                 }
00362         }
00363 
00364 
00365     // Ray-Segment Functor
00366     class RaySegmentIntersectionFunctor {
00367     public:
00368 
00369         template <class SEGMENTTYPE, class SCALARTYPE>
00370         inline bool operator () (const SEGMENTTYPE & S,
00371                                  const Ray2<SCALARTYPE> & ray,
00372                                  SCALARTYPE & t)
00373         {
00374             typedef SCALARTYPE ScalarType;
00375             typedef vcg::Point2<ScalarType> CoordType;
00376 
00377             CoordType inters_test;
00378             bool bret = RaySegmentIntersection(ray,S, inters_test);
00379             if (bret)
00380                 t=(inters_test-ray.Origin()).Norm();
00381             return (bret);
00382         }
00383     };
00384 
00386 } // end namespace
00387 #endif