shape_reconstruction: distance.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 #ifndef __VCGLIB_FACE_DISTANCE
00025 #define __VCGLIB_FACE_DISTANCE
00026 
00027 #include <vcg/math/base.h>
00028 #include <vcg/space/point3.h>
00029 #include <vcg/space/segment3.h>
00030 #include <vcg/space/distance3.h>
00031 
00032 namespace vcg {
00033     namespace face{
00034 /*
00035   Basic Wrapper for getting point-triangular face distance
00036   distance is unsigned;
00037 
00038   return true if the closest point <q> on <f> is nearer than the passed <dist>;
00039   return false otherwiswe (and q is not valid)
00040 
00041   This wrapper requires that your face has
00042   - Per Face Flags well initialized
00043   - Per Face EdgePlane component initialized.
00044   Initialization must be done with:
00045 
00046       tri::UpdateEdges<MeshType>::Set(yourMesh);
00047  */
00048     template <class FaceType>
00049     bool PointDistanceEP(       const FaceType &f,
00050                         const vcg::Point3<typename FaceType::ScalarType> & q,
00051                         typename FaceType::ScalarType & dist,
00052                         vcg::Point3<typename FaceType::ScalarType> & p )
00053     {
00054       typedef typename FaceType::ScalarType ScalarType;
00055       const ScalarType EPS = ScalarType( 0.000001);
00056 
00057         ScalarType b,b0,b1,b2;
00058 
00059         ScalarType d = SignedDistancePlanePoint( f.cPlane(), q );
00060         if( d>dist || d<-dist ) return false;
00061 
00062         Point3<ScalarType> t = f.cPlane().Direction();
00063         p = q - t*d; // p is the projection of q on the face plane
00064         // Now Choose the best plane and test to see if p is inside the triangle
00065 
00066         switch( f.cFlags() & (FaceType::NORMX|FaceType::NORMY|FaceType::NORMZ) )
00067         {
00068         case FaceType::NORMX:
00069             b0 = f.cEdge(1)[1]*(p[2] - f.cP(1)[2]) - f.cEdge(1)[2]*(p[1] - f.cP(1)[1]);
00070             if(b0<=0)
00071             {
00072                 b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00073                 if(dist>b0) { dist = b0; return true; }
00074                 else return false;
00075             }
00076             b1 = f.cEdge(2)[1]*(p[2] - f.cP(2)[2]) - f.cEdge(2)[2]*(p[1] - f.cP(2)[1]);
00077             if(b1<=0)
00078             {
00079                 b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00080                 if(dist>b1) { dist = b1; return true; }
00081                 else return false;
00082             }
00083             b2 = f.cEdge(0)[1]*(p[2] - f.cP(0)[2]) - f.cEdge(0)[2]*(p[1] - f.cP(0)[1]);
00084             if(b2<=0)
00085             {
00086                 b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
00087                 if(dist>b2) { dist = b2; return true; }
00088                 else return false;
00089             }
00090             // if all these tests failed the projection p should be inside.
00091             // Some further tests for more robustness...
00092             if( (b=std::min(b0,std::min(b1,b2)) ) < EPS*DoubleArea(f))
00093             {
00094               ScalarType bt;
00095               if(b==b0)             bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00096               else if(b==b1)    bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00097               else {          assert(b==b2);
00098                 bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
00099               }
00100               if(dist>bt) { dist = bt; return true; }
00101               else return false;
00102             }
00103             break;
00104 
00105           case FaceType::NORMY:
00106             b0 = f.cEdge(1)[2]*(p[0] - f.cP(1)[0]) - f.cEdge(1)[0]*(p[2] - f.cP(1)[2]);
00107             if(b0<=0)
00108             {
00109                 b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00110                 if(dist>b0) { dist = b0; return true; }
00111                 else return false;
00112             }
00113             b1 = f.cEdge(2)[2]*(p[0] - f.cP(2)[0]) - f.cEdge(2)[0]*(p[2] - f.cP(2)[2]);
00114             if(b1<=0)
00115             {
00116                 b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00117                 if(dist>b1) { dist = b1; return true; }
00118                 else return false;
00119             }
00120             b2 = f.cEdge(0)[2]*(p[0] - f.cP(0)[0]) - f.cEdge(0)[0]*(p[2] - f.cP(0)[2]);
00121             if(b2<=0)
00122             {
00123                 b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
00124                 if(dist>b2) { dist = b2; return true; }
00125                 else return false;
00126             }
00127             if( (b=math::Min<ScalarType>(b0,b1,b2)) < EPS*DoubleArea(f))
00128             {
00129               ScalarType bt;
00130               if(b==b0)             bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00131               else if(b==b1)    bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00132               else { assert(b==b2);
00133                 bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
00134               }
00135               if(dist>bt) { dist = bt; return true; }
00136                 else return false;
00137             }
00138             break;
00139 
00140           case FaceType::NORMZ:
00141             b0 = f.cEdge(1)[0]*(p[1] - f.cP(1)[1]) - f.cEdge(1)[1]*(p[0] - f.cP(1)[0]);
00142             if(b0<=0)
00143             {
00144                 b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00145                 if(dist>b0) { dist = b0; return true; }
00146                 else return false;
00147             }
00148             b1 = f.cEdge(2)[0]*(p[1] - f.cP(2)[1]) - f.cEdge(2)[1]*(p[0] - f.cP(2)[0]);
00149             if(b1<=0)
00150             {
00151                 b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00152                 if(dist>b1) { dist = b1; return true; }
00153                 else return false;
00154             }
00155             b2 = f.cEdge(0)[0]*(p[1] - f.cP(0)[1]) - f.cEdge(0)[1]*(p[0] - f.cP(0)[0]);
00156             if(b2<=0)
00157             {
00158                 b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
00159                 if(dist>b2) { dist = b2; return true; }
00160                 else return false;
00161             }
00162             if( (b=math::Min<ScalarType>(b0,b1,b2)) < EPS*DoubleArea(f))
00163             {
00164               ScalarType bt;
00165               if(b==b0)             bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00166               else if(b==b1)    bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00167               else { assert(b==b2);
00168                 bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
00169               }
00170 
00171               if(dist>bt) { dist = bt; return true; }
00172                 else return false;
00173             }
00174             break;
00175 
00176         } // end switch
00177 
00178         dist = ScalarType(fabs(d));
00179         return true;
00180     }
00181 
00182     template <class S>
00183     class PointDistanceEPFunctor {
00184     public:
00185         typedef S ScalarType;
00186         typedef Point3<ScalarType> QueryType;
00187         static inline const Point3<ScalarType> &  Pos(const QueryType & qt)  {return qt;}
00188 
00189         template <class FACETYPE, class SCALARTYPE>
00190         inline bool operator () (const FACETYPE & f, const Point3<SCALARTYPE> & p, SCALARTYPE & minDist, Point3<SCALARTYPE> & q) {
00191             const Point3<typename FACETYPE::ScalarType> fp = Point3<typename FACETYPE::ScalarType>::Construct(p);
00192             Point3<typename FACETYPE::ScalarType> fq;
00193             typename FACETYPE::ScalarType md = (typename FACETYPE::ScalarType)(minDist);
00194             const bool ret = PointDistanceEP(f, fp, md, fq);
00195             minDist = (SCALARTYPE)(md);
00196             q = Point3<SCALARTYPE>::Construct(fq);
00197             return (ret);
00198         }
00199     };
00200 
00201 
00202 
00203     template <class S>
00204     class PointNormalDistanceFunctor {
00205     public:
00206         typedef typename S::ScalarType ScalarType;
00207         typedef S QueryType;
00208         static inline const Point3<ScalarType> &  Pos(const QueryType & qt)  {return qt.P();}
00209 
00210 
00211         static ScalarType & Alpha(){static ScalarType alpha = 1.0; return alpha;}
00212         static ScalarType & Beta (){static ScalarType beta  = 1.0; return beta;}
00213         static ScalarType & Gamma(){static ScalarType gamma = 1.0; return gamma;}
00214         static ScalarType & InterPoint(){static ScalarType interpoint= 1.0; return interpoint;}
00215 
00216 
00217         template <class FACETYPE, class SCALARTYPE>
00218         inline bool operator () (const FACETYPE &f, const typename FACETYPE::VertexType &p,
00219             SCALARTYPE & minDist,Point3<SCALARTYPE> & q)
00220         {
00221             const Point3<typename FACETYPE::ScalarType> fp = Point3<typename FACETYPE::ScalarType>::Construct(p.cP());
00222             const Point3<typename FACETYPE::ScalarType> fn = Point3<typename FACETYPE::ScalarType>::Construct(p.cN());
00223             Point3<typename FACETYPE::ScalarType> fq;
00224             typename FACETYPE::ScalarType md = (typename FACETYPE::ScalarType)(minDist);
00225             const bool ret=PointDistance(f,fp,md,fq);
00226 
00227             SCALARTYPE  dev=InterPoint()*(pow((ScalarType)(1-f.cN().dot(fn)),(ScalarType)Beta())/(Gamma()*md+0.1));
00228 
00229             if (md+dev < minDist){
00230                 minDist = (SCALARTYPE)(md+dev);
00231                 q = Point3<SCALARTYPE>::Construct(fq);
00232                 //q.N() = f.N();
00233                 return (ret);
00234             }
00235             return false;
00236         }
00237     };
00238 
00241 
00242         template <class FaceType>
00243             bool PointDistanceBase(
00244                                                     const FaceType &f,                                                                                                                                          
00245                                                     const vcg::Point3<typename FaceType::ScalarType> & q, 
00246                                                     typename FaceType::ScalarType & dist,                 
00247                                                     vcg::Point3<typename FaceType::ScalarType> & p )
00248         {
00249                 typedef typename FaceType::ScalarType ScalarType;
00250 
00251                 if(f.cN()==Point3<ScalarType>(0,0,0)) // to correctly manage the case of degenerate triangles we consider them as segments.
00252                 {
00253                   Box3<ScalarType> bb;
00254                   f.GetBBox(bb);
00255                   Segment3<ScalarType> degenTri(bb.min,bb.max);
00256                   Point3<ScalarType> closest;
00257                   ScalarType d;
00258                   if(bb.Diag()>0)
00259                     vcg::SegmentPointDistance<ScalarType>(degenTri,q,closest,d);
00260                   else // very degenerate triangle (just a point)
00261                   {
00262                     closest = bb.min;
00263                     d=Distance(q,closest);
00264                   }
00265                   if( d>dist) return false;
00266                   dist=d;
00267                   p=closest;
00268                   assert(!math::IsNAN(dist));
00269                   return true;
00270                 }
00271 
00272                 Plane3<ScalarType,true> fPlane;
00273                 fPlane.Init(f.cP(0),f.cN());
00274                 const ScalarType EPS = ScalarType( 0.000001);
00275                 ScalarType b,b0,b1,b2;
00276                 // Calcolo distanza punto piano
00277                 ScalarType d = SignedDistancePlanePoint( fPlane, q );
00278                 if( d>dist || d<-dist )                 // Risultato peggiore: niente di fatto
00279                     return false;
00280 
00281                 // Projection of query point onto the triangle plane
00282                 p = q - fPlane.Direction()*d;
00283 
00284                 Point3<ScalarType> fEdge[3];
00285                 fEdge[0] = f.cP(1); fEdge[0] -= f.cP(0);
00286                 fEdge[1] = f.cP(2); fEdge[1] -= f.cP(1);
00287                 fEdge[2] = f.cP(0); fEdge[2] -= f.cP(2);
00288 
00289                 /*
00290                 This piece of code is part of the EdgePlane initialization structure: note that the edges are scaled!.
00291 
00292                 if(nx>ny && nx>nz) { f.Flags() |= FaceType::NORMX; d = 1/f.Plane().Direction()[0]; }
00293                 else if(ny>nz)     { f.Flags() |= FaceType::NORMY; d = 1/f.Plane().Direction()[1]; }
00294                 else               { f.Flags() |= FaceType::NORMZ; d = 1/f.Plane().Direction()[2]; }
00295                 f.Edge(0)*=d; f.Edge(1)*=d;f.Edge(2)*=d;
00296 
00297                 So we must apply the same scaling according to the plane orientation, eg in the case of NORMX
00298                   scaleFactor= 1/fPlane.Direction()[0];
00299                   fEdge[0]*=d; fEdge[1]*=d;fEdge[2]*=d;
00300                 */
00301 
00302                 int bestAxis;
00303                 if(fabs(f.cN()[0])>fabs(f.cN()[1]))
00304                 {
00305                   if(fabs(f.cN()[0])>fabs(f.cN()[2])) bestAxis = 0;
00306                   else bestAxis = 2;
00307                 } else {
00308                   if(fabs(f.cN()[1])>fabs(f.cN()[2])) bestAxis=1; /* 1 > 0 ? 2 */
00309                   else bestAxis=2; /* 2 > 1 ? 2 */
00310                 }
00311 
00312                 ScalarType scaleFactor;
00313 
00314                 switch( bestAxis )
00315                 {
00316                     case 0:  /************* X AXIS **************/
00317                         scaleFactor= 1/fPlane.Direction()[0];
00318                         fEdge[0]*=scaleFactor; fEdge[1]*=scaleFactor; fEdge[2]*=scaleFactor;
00319 
00320                         b0 = fEdge[1][1]*(p[2] - f.cP(1)[2]) - fEdge[1][2]*(p[1] - f.cP(1)[1]);
00321                         if(b0<=0)
00322                         {
00323                             b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00324                             if(dist>b0) { dist = b0; return true; }
00325                             else return false;
00326                         }
00327                             b1 = fEdge[2][1]*(p[2] - f.cP(2)[2]) - fEdge[2][2]*(p[1] - f.cP(2)[1]);
00328                         if(b1<=0)
00329                         {
00330                             b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00331                             if(dist>b1) { dist = b1; return true; }
00332                             else return false;
00333                         }
00334                             b2 = fEdge[0][1]*(p[2] - f.cP(0)[2]) - fEdge[0][2]*(p[1] - f.cP(0)[1]);
00335                         if(b2<=0)
00336                         {
00337                             b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
00338                             if(dist>b2) { dist = b2; return true; }
00339                             else return false;
00340                         }
00341                             // sono tutti e tre > 0 quindi dovrebbe essere dentro;
00342                             // per sicurezza se il piu' piccolo dei tre e' < epsilon (scalato rispetto all'area della faccia
00343                             // per renderlo dimension independent.) allora si usa ancora la distanza punto
00344                             // segmento che e' piu robusta della punto piano, e si fa dalla parte a cui siamo piu'
00345                             // vicini (come prodotto vettore)
00346                             // Nota: si potrebbe rendere un pochino piu' veloce sostituendo Area()
00347                             // con il prodotto vettore dei due edge in 2d lungo il piano migliore.
00348                         if( (b=vcg::math::Min<ScalarType>(b0,b1,b2)) < EPS*DoubleArea(f))
00349                             {
00350                                 ScalarType bt;
00351                                 if(b==b0)           bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00352                                 else if(b==b1)  bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00353                                 else {assert(b==b2); bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);}
00354                                 //printf("Warning area:%g %g %g %g thr:%g bt:%g\n",Area(), b0,b1,b2,EPSILON*Area(),bt);
00355                                 if(dist>bt) { dist = bt; return true; }
00356                                 else return false;
00357                             }
00358                             break;
00359 
00360                     case 1:  /************* Y AXIS **************/
00361                         scaleFactor= 1/fPlane.Direction()[1];
00362                         fEdge[0]*=scaleFactor; fEdge[1]*=scaleFactor; fEdge[2]*=scaleFactor;
00363 
00364                         b0 = fEdge[1][2]*(p[0] - f.cP(1)[0]) - fEdge[1][0]*(p[2] - f.cP(1)[2]);
00365                         if(b0<=0)
00366                         {
00367                             b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00368                             if(dist>b0) { dist = b0; return true; }
00369                             else return false;
00370                         }
00371                             b1 = fEdge[2][2]*(p[0] - f.cP(2)[0]) - fEdge[2][0]*(p[2] - f.cP(2)[2]);
00372                         if(b1<=0)
00373                         {
00374                             b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00375                             if(dist>b1) { dist = b1; return true; }
00376                             else return false;
00377                         }
00378                             b2 = fEdge[0][2]*(p[0] - f.cP(0)[0]) - fEdge[0][0]*(p[2] - f.cP(0)[2]);
00379                         if(b2<=0)
00380                         {
00381                             b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
00382                             if(dist>b2) { dist = b2; return true; }
00383                             else return false;
00384                         }
00385             if( (b=vcg::math::Min<ScalarType>(b0,b1,b2)) < EPS*DoubleArea(f))
00386                             {
00387                                 ScalarType bt;
00388                                 if(b==b0)           bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00389                                 else if(b==b1)  bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00390                                 else{ assert(b==b2); bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);}
00391                                 //printf("Warning area:%g %g %g %g thr:%g bt:%g\n",Area(), b0,b1,b2,EPSILON*Area(),bt);
00392                                 if(dist>bt) { dist = bt; return true; }
00393                                 else return false;
00394                             }
00395                             break;
00396 
00397                     case 2:  /************* Z AXIS **************/
00398                         scaleFactor= 1/fPlane.Direction()[2];
00399                         fEdge[0]*=scaleFactor; fEdge[1]*=scaleFactor; fEdge[2]*=scaleFactor;
00400 
00401                         b0 = fEdge[1][0]*(p[1] - f.cP(1)[1]) - fEdge[1][1]*(p[0] - f.cP(1)[0]);
00402                         if(b0<=0)
00403                         {
00404                             b0 = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00405                             if(dist>b0) { dist = b0; return true; }
00406                             else return false;
00407                         }
00408                             b1 = fEdge[2][0]*(p[1] - f.cP(2)[1]) - fEdge[2][1]*(p[0] - f.cP(2)[0]);
00409                         if(b1<=0)
00410                         {
00411                             b1 = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00412                             if(dist>b1) { dist = b1; return true; }
00413                             else return false;
00414                         }
00415                             b2 = fEdge[0][0]*(p[1] - f.cP(0)[1]) - fEdge[0][1]*(p[0] - f.cP(0)[0]);
00416                         if(b2<=0)
00417                         {
00418                             b2 = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p);
00419                             if(dist>b2) { dist = b2; return true; }
00420                             else return false;
00421                         }
00422             if( (b=vcg::math::Min<ScalarType>(b0,b1,b2)) < EPS*DoubleArea(f))
00423                             {
00424                                 ScalarType bt;
00425                                 if(b==b0)           bt = PSDist(q,f.cV(1)->cP(),f.cV(2)->cP(),p);
00426                                 else if(b==b1)  bt = PSDist(q,f.cV(2)->cP(),f.cV(0)->cP(),p);
00427                                 else { assert(b==b2); bt = PSDist(q,f.cV(0)->cP(),f.cV(1)->cP(),p); }
00428                                 //printf("Warning area:%g %g %g %g thr:%g bt:%g\n",Area(), b0,b1,b2,EPSILON*Area(),bt);
00429 
00430                                 if(dist>bt) { dist = bt; return true; }
00431                                 else return false;
00432                             }
00433                             break;
00434                         default: assert(0); // if you get this assert it means that you forgot to set the required UpdateFlags<MeshType>::FaceProjection(m);
00435 
00436                 }
00437 
00438                 dist = ScalarType(fabs(d));
00439                 //dist = Distance(p,q);
00440                 return true;
00441         }
00442 
00443     template <class S>
00444     class PointDistanceBaseFunctor {
00445 public:
00446             typedef S ScalarType;
00447             typedef Point3<ScalarType> QueryType;
00448 
00449           static inline const Point3<ScalarType> & Pos(const Point3<ScalarType> & qt)  {return qt;}
00450             template <class FACETYPE, class SCALARTYPE>
00451             inline bool operator () (const FACETYPE & f, const Point3<SCALARTYPE> & p, SCALARTYPE & minDist, Point3<SCALARTYPE> & q) {
00452                 const Point3<typename FACETYPE::ScalarType> fp = Point3<typename FACETYPE::ScalarType>::Construct(p);
00453                 Point3<typename FACETYPE::ScalarType> fq;
00454                 typename FACETYPE::ScalarType md = (typename FACETYPE::ScalarType)(minDist);
00455                 const bool ret = PointDistanceBase(f, fp, md, fq);
00456                 minDist = (SCALARTYPE)(md);
00457                 q = Point3<SCALARTYPE>::Construct(fq);
00458                 return (ret);
00459             }
00460         };
00461 
00462 
00463 
00464 }        // end namespace face
00465 
00466 }        // end namespace vcg
00467 
00468 
00469 #endif
00470