diff --git a/vcg/simplex/face/distance.h b/vcg/simplex/face/distance.h index ed9861b8..443ec86a 100644 --- a/vcg/simplex/face/distance.h +++ b/vcg/simplex/face/distance.h @@ -248,6 +248,205 @@ namespace vcg { } }; + + /// BASIC VERSION of the Point-face distance that does not require the EdgePlane Additional data. + /// Given a face and a point, returns the closest point of the face to p. + /// it assumes that the face has Normalized Normal and on the flags stored the preferred orientation. + // UpdateNormals::PerFaceNormalized(m) + // UpdateFlags<>::FaceProjection(m); + + template + bool PointDistanceBase( + const FaceType &f, /// the face to be tested + const vcg::Point3 & q, /// the point tested + typename FaceType::ScalarType & dist, /// bailout distance. It must be initialized with the max admittable value. + vcg::Point3 & p ) + { + typedef typename FaceType::ScalarType ScalarType; + Plane3 fPlane; + fPlane.Init(f.cP(0),f.cN()); + const ScalarType EPSILON = ScalarType( 0.000001); + ScalarType b,b0,b1,b2; + // Calcolo distanza punto piano + ScalarType d = Distance( fPlane, q ); + if( d>dist || d<-dist ) // Risultato peggiore: niente di fatto + return false; + + // Calcolo del punto sul piano + // NOTA: aggiunto un '-d' in fondo Paolo C. + Point3 t = fPlane.Direction(); + t[0] *= -d; + t[1] *= -d; + t[2] *= -d; + p = q; p += t; + + Point3 fEdge[3]; + fEdge[0] = f.cP(1); fEdge[0] -= f.cP(0); + fEdge[1] = f.cP(2); fEdge[1] -= f.cP(1); + fEdge[2] = f.cP(0); fEdge[2] -= f.cP(2); + + /* + This piece of code is part of the EdgePlane initialization structure: note that the edges are scaled!. + + if(nx>ny && nx>nz) { f.Flags() |= FaceType::NORMX; d = 1/f.Plane().Direction()[0]; } + else if(ny>nz) { f.Flags() |= FaceType::NORMY; d = 1/f.Plane().Direction()[1]; } + else { f.Flags() |= FaceType::NORMZ; d = 1/f.Plane().Direction()[2]; } + f.Edge(0)*=d; f.Edge(1)*=d;f.Edge(2)*=d; + + So we must apply the same scaling according to the plane orientation, eg in the case of NORMX + + scaleFactor= 1/fPlane.Direction()[0]; + fEdge[0]*=d; fEdge[1]*=d;fEdge[2]*=d; + */ + + ScalarType scaleFactor; + + switch( f.Flags() & (FaceType::NORMX|FaceType::NORMY|FaceType::NORMZ) ) + { + case FaceType::NORMX: + scaleFactor= 1/fPlane.Direction()[0]; + fEdge[0]*=d; fEdge[1]*=d;fEdge[2]*=d; + + b0 = fEdge[1][1]*(p[2] - f.cP(1)[2]) - fEdge[1][2]*(p[1] - f.cP(1)[1]); + if(b0<=0) + { + b0 = PSDist(q,f.V(1)->cP(),f.V(2)->cP(),p); + if(dist>b0) { dist = b0; return true; } + else return false; + } + b1 = fEdge[2][1]*(p[2] - f.cP(2)[2]) - fEdge[2][2]*(p[1] - f.cP(2)[1]); + if(b1<=0) + { + b1 = PSDist(q,f.V(2)->cP(),f.V(0)->cP(),p); + if(dist>b1) { dist = b1; return true; } + else return false; + } + b2 = fEdge[0][1]*(p[2] - f.cP(0)[2]) - fEdge[0][2]*(p[1] - f.cP(0)[1]); + if(b2<=0) + { + b2 = PSDist(q,f.V(0)->cP(),f.V(1)->cP(),p); + if(dist>b2) { dist = b2; return true; } + else return false; + } + // sono tutti e tre > 0 quindi dovrebbe essere dentro; + // per sicurezza se il piu' piccolo dei tre e' < epsilon (scalato rispetto all'area della faccia + // per renderlo dimension independent.) allora si usa ancora la distanza punto + // segmento che e' piu robusta della punto piano, e si fa dalla parte a cui siamo piu' + // vicini (come prodotto vettore) + // Nota: si potrebbe rendere un pochino piu' veloce sostituendo Area() + // con il prodotto vettore dei due edge in 2d lungo il piano migliore. + if( (b=vcg::math::Min(b0,vcg::math::Min(b1,b2))) < EPSILON*DoubleArea(f)) + { + ScalarType bt; + if(b==b0) bt = PSDist(q,f.V(1)->cP(),f.V(2)->cP(),p); + else if(b==b1) bt = PSDist(q,f.V(2)->cP(),f.V(0)->cP(),p); + else if(b==b2) bt = PSDist(q,f.V(0)->cP(),f.V(1)->cP(),p); + //printf("Warning area:%g %g %g %g thr:%g bt:%g\n",Area(), b0,b1,b2,EPSILON*Area(),bt); + if(dist>bt) { dist = bt; return true; } + else return false; + } + break; + + case FaceType::NORMY: + scaleFactor= 1/fPlane.Direction()[1]; + fEdge[0]*=d; fEdge[1]*=d;fEdge[2]*=d; + + b0 = fEdge[1][2]*(p[0] - f.cP(1)[0]) - fEdge[1][0]*(p[2] - f.cP(1)[2]); + if(b0<=0) + { + b0 = PSDist(q,f.V(1)->cP(),f.V(2)->cP(),p); + if(dist>b0) { dist = b0; return true; } + else return false; + } + b1 = fEdge[2][2]*(p[0] - f.cP(2)[0]) - fEdge[2][0]*(p[2] - f.cP(2)[2]); + if(b1<=0) + { + b1 = PSDist(q,f.V(2)->cP(),f.V(0)->cP(),p); + if(dist>b1) { dist = b1; return true; } + else return false; + } + b2 = fEdge[0][2]*(p[0] - f.cP(0)[0]) - fEdge[0][0]*(p[2] - f.cP(0)[2]); + if(b2<=0) + { + b2 = PSDist(q,f.V(0)->cP(),f.V(1)->cP(),p); + if(dist>b2) { dist = b2; return true; } + else return false; + } + if( (b=vcg::math::Min(b0,vcg::math::Min(b1,b2))) < EPSILON*DoubleArea(f)) + { + ScalarType bt; + if(b==b0) bt = PSDist(q,f.V(1)->cP(),f.V(2)->cP(),p); + else if(b==b1) bt = PSDist(q,f.V(2)->cP(),f.V(0)->cP(),p); + else if(b==b2) bt = PSDist(q,f.V(0)->cP(),f.V(1)->cP(),p); + //printf("Warning area:%g %g %g %g thr:%g bt:%g\n",Area(), b0,b1,b2,EPSILON*Area(),bt); + if(dist>bt) { dist = bt; return true; } + else return false; + } + break; + + case FaceType::NORMZ: + scaleFactor= 1/fPlane.Direction()[2]; + fEdge[0]*=d; fEdge[1]*=d;fEdge[2]*=d; + + b0 = fEdge[1][0]*(p[1] - f.cP(1)[1]) - fEdge[1][1]*(p[0] - f.cP(1)[0]); + if(b0<=0) + { + b0 = PSDist(q,f.V(1)->cP(),f.V(2)->cP(),p); + if(dist>b0) { dist = b0; return true; } + else return false; + } + b1 = fEdge[2][0]*(p[1] - f.cP(2)[1]) - fEdge[2][1]*(p[0] - f.cP(2)[0]); + if(b1<=0) + { + b1 = PSDist(q,f.V(2)->cP(),f.V(0)->cP(),p); + if(dist>b1) { dist = b1; return true; } + else return false; + } + b2 = fEdge[0][0]*(p[1] - f.cP(0)[1]) - fEdge[0][1]*(p[0] - f.cP(0)[0]); + if(b2<=0) + { + b2 = PSDist(q,f.V(0)->cP(),f.V(1)->cP(),p); + if(dist>b2) { dist = b2; return true; } + else return false; + } + if( (b=vcg::math::Min(b0,vcg::math::Min(b1,b2))) < EPSILON*DoubleArea(f)) + { + ScalarType bt; + if(b==b0) bt = PSDist(q,f.V(1)->cP(),f.V(2)->cP(),p); + else if(b==b1) bt = PSDist(q,f.V(2)->cP(),f.V(0)->cP(),p); + else if(b==b2) bt = PSDist(q,f.V(0)->cP(),f.V(1)->cP(),p); + //printf("Warning area:%g %g %g %g thr:%g bt:%g\n",Area(), b0,b1,b2,EPSILON*Area(),bt); + + if(dist>bt) { dist = bt; return true; } + else return false; + } + break; + + } + + dist = ScalarType(fabs(d)); + //dist = Distance(p,q); + return true; + } + + + + class PointDistanceBaseFunctor { +public: + template + inline bool operator () (const FACETYPE & f, const Point3 & p, SCALARTYPE & minDist, Point3 & q) { + const Point3 fp = Point3::Construct(p); + Point3 fq; + typename FACETYPE::ScalarType md = (typename FACETYPE::ScalarType)(minDist); + const bool ret = PointDistanceBase(f, fp, md, fq); + minDist = (SCALARTYPE)(md); + q = Point3::Construct(fq); + return (ret); + } + }; + + + } // end namespace face } // end namespace vcg