diff --git a/vcg/math/quadric.h b/vcg/math/quadric.h index a4cfb4f4..45778a29 100644 --- a/vcg/math/quadric.h +++ b/vcg/math/quadric.h @@ -197,135 +197,32 @@ public: } + template + bool MinimumClosestToPoint(Point3 &x, const Point3 &pt) + { + const double qeps = 1e-3; + Eigen::Matrix3d A; + Eigen::Vector3d be; + A << a[0], a[1], a[2], + a[1], a[3], a[4], + a[2], a[4], a[5]; + be << -b[0]/2, -b[1]/2, -b[2]/2; - - -// spostare..risolve un sistema 3x3 -template -bool Gauss33( FLTYPE x[], FLTYPE C[3][3+1] ) -{ - const FLTYPE keps = (FLTYPE)1e-3; - int i,j,k; - - FLTYPE eps; // Determina valore cond. - eps = math::Abs(C[0][0]); - for(i=1;i<3;++i) - { - FLTYPE t = math::Abs(C[i][i]); - if( eps vma) - { - vma = t; - ma = k; - } - } - if (vma=0; i--) // Sostituzione - { - FLTYPE t; - for (t = 0.0, j = i + 1; j < 3; j++) - t += C[i][j] * x[j]; - x[i] = (C[i][3] - t) / C[i][i]; - } + Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::Vector3d s = svd.singularValues(); + for(int i=1;i<3;++i) + if(s[i]/s[0] > qeps) s[i]=1/s[i]; + else s[i]=0; + s[0]=1/s[0]; + + Eigen::Vector3d xp; + pt.ToEigenVector(xp); + Eigen::Vector3d xe = xp + (svd.matrixV()*s.asDiagonal()*(svd.matrixU().transpose())) *(be - A*xp); + x.FromEigenVector(xe); return true; -} - - - -template -bool MinimumOld(Point3 &x) -{ - ReturnScalarType C[3][4]; - C[0][0]=a[0]; C[0][1]=a[1]; C[0][2]=a[2]; - C[1][0]=a[1]; C[1][1]=a[3]; C[1][2]=a[4]; - C[2][0]=a[2]; C[2][1]=a[4]; C[2][2]=a[5]; - - C[0][3]=-b[0]/2; - C[1][3]=-b[1]/2; - C[2][3]=-b[2]/2; - return Gauss33(&(x[0]),C); -} - -// determina il punto di errore minimo vincolato nel segmento (a,b) -bool Minimum(Point3 &x,Point3 &pa,Point3 &pb){ -ScalarType t1,t2, t4, t5, t8, t9, - t11,t12,t14,t15,t17,t18,t25,t26,t30,t34,t35, - t41,t42,t44,t45,t50,t52,t54, - t56,t21,t23,t37,t64,lambda; - - t1 = a[4]*pb.z(); - t2 = t1*pa.y(); - t4 = a[1]*pb.y(); - t5 = t4*pa.x(); - t8 = a[1]*pa.y(); - t9 = t8*pa.x(); - t11 = a[4]*pa.z(); - t12 = t11*pa.y(); - t14 = pa.z()*pa.z(); - t15 = a[5]*t14; - t17 = a[2]*pa.z(); - t18 = t17*pa.x(); - t21 = 2.0*t11*pb.y(); - t23 = a[5]*pb.z()*pa.z(); - t25 = a[2]*pb.z(); - t26 = t25*pa.x(); - t30 = a[0]*pb.x()*pa.x(); - t34 = 2.0*a[3]*pb.y()*pa.y(); - t35 = t17*pb.x(); - t37 = t8*pb.x(); - t41 = pa.x()*pa.x(); - t42 = a[0]*t41; - t44 = pa.y()*pa.y(); - t45 = a[3]*t44; - t50 = 2.0*t30+t34+2.0*t35+2.0*t37-(-b[2]/2)*pa.z()-(-b[0]/2)*pa.x()-2.0*t42-2.0*t45+(-b[1]/2)*pb.y() -+(-b[0]/2)*pb.x()-(-b[1]/2)*pa.y(); - t52 = pb.y()*pb.y(); - t54 = pb.z()*pb.z(); - t56 = pb.x()*pb.x(); - t64 = t5+t37-t9+t30-t18+t35+t26-t25*pb.x()+t2-t1*pb.y()+t23; - lambda = (2.0*t2+2.0*t5+(-b[2]/2)*pb.z()-4.0*t9-4.0*t12-2.0*t15-4.0*t18+t21+2.0*t23+ -2.0*t26+t50)/(-t45-a[3]*t52-a[5]*t54-a[0]*t56-t15-t42+t34-2.0*t12+t21-2.0*t4*pb.x()+ -2.0*t64)/2.0; - - if(lambda<0) lambda=0; else if(lambda>1) lambda = 1; - - x = pa*(1.0-lambda)+pb*lambda; - return true; - } - + } + }; typedef Quadric Quadrics;