vcglib: quadric.h Source 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.7  2006/11/13 12:53:40  ponchio
00028 just added an #include <matrix33>
00029 
00030 Revision 1.6  2006/10/09 20:23:00  cignoni
00031 Added a minimum method that uses SVD. Unfortunately it is much much slower.
00032 
00033 Revision 1.5  2004/12/10 01:31:59  cignoni
00034 added an alternative QuadricMinimization (we should use LRU decomposition!!)
00035 
00036 Revision 1.3  2004/10/25 16:23:51  ponchio
00037 typedef ScalarType ScalarType; was a problem on g++
00038 
00039 Revision 1.2  2004/10/25 16:15:59  ganovelli
00040 template changed
00041 
00042 Revision 1.1  2004/09/14 19:48:27  ganovelli
00043 created
00044 
00045 
00046 ****************************************************************************/
00047 #ifndef __VCGLIB_QUADRIC
00048 #define __VCGLIB_QUADRIC
00049 
00050 #include <vcg/space/point3.h>
00051 #include <vcg/space/plane3.h>
00052 #include <vcg/math/matrix33.h>
00053 
00054 namespace vcg {
00055 namespace math {
00056 
00057 
00058 template<typename  Scalar>
00059 class Quadric
00060 {
00061 public:
00062         typedef Scalar ScalarType;
00063         ScalarType a[6];                // Matrice 3x3 simmetrica: a11 a12 a13 a22 a23 a33
00064         ScalarType b[3];                // Vettore r3
00065         ScalarType c;                   // Fattore scalare (se -1 quadrica nulla)
00066 
00067         inline Quadric() { c = -1; }
00068 
00069         // Necessari se si utilizza stl microsoft
00070         // inline bool operator <  ( const Quadric & q ) const { return false; }
00071         // inline bool operator == ( const Quadric & q ) const { return true; }
00072 
00073         bool IsValid() const { return c>=0; }
00074         void SetInvalid() { c = -1.0; }
00075 
00076 template< class PlaneType >
00077         void ByPlane( const PlaneType & p )                                     // Init dato un piano
00078         {
00079                 a[0] =  (ScalarType)p.Direction()[0]*p.Direction()[0];  // a11
00080                 a[1] =  (ScalarType)p.Direction()[1]*p.Direction()[0];  // a12 (=a21)
00081                 a[2] =  (ScalarType)p.Direction()[2]*p.Direction()[0];  // a13 (=a31)
00082                 a[3] =  (ScalarType)p.Direction()[1]*p.Direction()[1];  // a22
00083                 a[4] =  (ScalarType)p.Direction()[2]*p.Direction()[1];  // a23 (=a32)
00084                 a[5] =  (ScalarType)p.Direction()[2]*p.Direction()[2];  // a33
00085                 b[0] = (ScalarType)(-2.0)*p.Offset()*p.Direction()[0];
00086                 b[1] = (ScalarType)(-2.0)*p.Offset()*p.Direction()[1];
00087                 b[2] = (ScalarType)(-2.0)*p.Offset()*p.Direction()[2];
00088                 c    =  (ScalarType)p.Offset()*p.Offset();
00089         }
00090 
00091 /* Initializes the quadric as the squared distance from a given line.
00092    Notice that this code also works for a vcg::Ray<T>, even though the (squared) distance
00093    from a ray is different "before" its origin.
00094  */
00095  template< class LineType >
00096   void ByLine( const LineType & r ) // Init dato un raggio
00097   {
00098     ScalarType K = (ScalarType)(r.Origin()*r.Direction());
00099     a[0] = (ScalarType)1.0-r.Direction()[0]*r.Direction()[0]; // a11
00100     a[1] = (ScalarType)-r.Direction()[0]*r.Direction()[1]; // a12 (=a21)
00101     a[2] = (ScalarType)-r.Direction()[0]*r.Direction()[2]; // a13 (=a31)
00102     a[3] = (ScalarType)1.0-r.Direction()[1]*r.Direction()[1]; // a22
00103     a[4] = (ScalarType)-r.Direction()[1]*r.Direction()[2]; // a23 (=a32)
00104     a[5] = (ScalarType)1.0-r.Direction()[2]*r.Direction()[2]; // a33
00105     b[0] = (ScalarType)2.0*(r.Direction()[0]*K - r.Origin()[0]);
00106     b[1] = (ScalarType)2.0*(r.Direction()[1]*K - r.Origin()[1]);
00107     b[2] = (ScalarType)2.0*(r.Direction()[2]*K - r.Origin()[2]);
00108     c = -K*K + (ScalarType)(r.Origin()*r.Origin());
00109   }
00110 
00111         void SetZero()                                                                                                                          // Azzera la quadrica
00112         {
00113                 a[0] = 0;
00114                 a[1] = 0;
00115                 a[2] = 0;
00116                 a[3] = 0;
00117                 a[4] = 0;
00118                 a[5] = 0;
00119                 b[0] = 0;
00120                 b[1] = 0;
00121                 b[2] = 0;
00122                 c    = 0;
00123         }
00124 
00125 void operator = ( const Quadric & q )                   // Assegna una quadrica
00126         {
00127                 //assert( IsValid() );
00128                 assert( q.IsValid() );
00129 
00130                 a[0] = q.a[0];
00131                 a[1] = q.a[1];
00132                 a[2] = q.a[2];
00133                 a[3] = q.a[3];
00134                 a[4] = q.a[4];
00135                 a[5] = q.a[5];
00136                 b[0] = q.b[0];
00137                 b[1] = q.b[1];
00138                 b[2] = q.b[2];
00139                 c    = q.c;
00140         }
00141 
00142   void operator += ( const Quadric & q )                        // Somma una quadrica
00143         {
00144                 assert( IsValid() );
00145                 assert( q.IsValid() );
00146 
00147                 a[0] += q.a[0];
00148                 a[1] += q.a[1];
00149                 a[2] += q.a[2];
00150                 a[3] += q.a[3];
00151                 a[4] += q.a[4];
00152                 a[5] += q.a[5];
00153                 b[0] += q.b[0];
00154                 b[1] += q.b[1];
00155                 b[2] += q.b[2];
00156                 c    += q.c;
00157         }
00158 
00159 template <class ResultScalarType>
00160         ResultScalarType Apply( const Point3<ResultScalarType> & p ) const      // Applica la quadrica al punto p
00161         {
00162                 assert( IsValid() );
00163 /*
00164         // Versione Lenta
00165 
00166                 Point3d t;
00167                 t[0] = p[0]*a[0] + p[1]*a[1] + p[2]*a[2];
00168                 t[1] = p[0]*a[1] + p[1]*a[3] + p[2]*a[4];
00169                 t[2] = p[0]*a[2] + p[1]*a[4] + p[2]*a[5];
00170                 double k = b[0]*p[0] + b[1]*p[1] + b[2]*p[2];
00171                 double tp = t*p ;
00172                 assert(tp+k+c >= 0);
00173                 return tp + k + c;
00174         */
00175 
00176         /* Versione veloce */
00177                 return ResultScalarType (
00178       p[0]*p[0]*a[0] + 2*p[0]*p[1]*a[1] + 2*p[0]*p[2]*a[2] + p[0]*b[0]
00179                                        +   p[1]*p[1]*a[3] + 2*p[1]*p[2]*a[4] + p[1]*b[1]
00180                                                           +   p[2]*p[2]*a[5] + p[2]*b[2]        + c);
00181 
00182         }
00183 
00184 // spostare..risolve un sistema 3x3
00185 template<class FLTYPE>
00186 bool Gauss33( FLTYPE x[], FLTYPE C[3][3+1] )
00187 {
00188     const FLTYPE keps = (FLTYPE)1e-3;
00189     int i,j,k;
00190 
00191     FLTYPE eps;                                 // Determina valore cond.
00192                 eps = math::Abs(C[0][0]);
00193     for(i=1;i<3;++i)
00194     {
00195                 FLTYPE t = math::Abs(C[i][i]);
00196                 if( eps<t ) eps = t;
00197     }
00198     eps *= keps;
00199 
00200     for (i = 0; i < 3 - 1; ++i)                 // Ciclo di riduzione
00201     {
00202         int ma = i;                             // Ricerca massimo pivot
00203         FLTYPE vma = math::Abs( C[i][i] );
00204         for (k = i + 1; k < 3; k++)
00205         {
00206             FLTYPE t = math::Abs( C[k][i] );
00207             if (t > vma)
00208             {
00209                 vma = t;
00210                 ma  = k;
00211             }
00212         }
00213         if (vma<eps)
00214             return false;                               // Matrice singolare
00215         if(i!=ma)                               // Swap del massimo pivot
00216             for(k=0;k<=3;k++)
00217             {
00218                 FLTYPE t = C[i][k];
00219                 C[i][k] = C[ma][k];
00220                 C[ma][k] = t;
00221             }
00222         for (k = i + 1; k < 3; k++)             //  Riduzione
00223         {
00224             FLTYPE s;
00225             s = C[k][i] / C[i][i];
00226             for (j = i+1; j <= 3; j++)
00227                 C[k][j] -= C[i][j] * s;
00228             C[k][i] = 0.0;
00229         }
00230     }
00231 
00232         // Controllo finale singolarita'
00233     if( math::Abs(C[3-1][3- 1])<eps)
00234         return false;
00235 
00236     for (i=3-1; i>=0; i--)                      // Sostituzione
00237     {
00238         FLTYPE t;
00239         for (t = 0.0, j = i + 1; j < 3; j++)
00240             t += C[i][j] * x[j];
00241         x[i] = (C[i][3] - t) / C[i][i];
00242     }
00243 
00244     return true;
00245 }
00246 
00247 // determina il punto di errore minimo
00248 template <class ReturnScalarType>
00249 bool Minimum(Point3<ReturnScalarType> &x)
00250 {
00251                 ReturnScalarType C[3][4];
00252                 C[0][0]=a[0]; C[0][1]=a[1]; C[0][2]=a[2];
00253                 C[1][0]=a[1]; C[1][1]=a[3]; C[1][2]=a[4];
00254                 C[2][0]=a[2]; C[2][1]=a[4]; C[2][2]=a[5];
00255 
00256                 C[0][3]=-b[0]/2;
00257                 C[1][3]=-b[1]/2;
00258                 C[2][3]=-b[2]/2;
00259                 return Gauss33(&(x[0]),C);
00260 }
00261 
00262 // determina il punto di errore minimo usando le fun di inversione di matrice che usano svd
00263 // Molto + lento
00264 template <class ReturnScalarType>
00265 bool MinimumSVD(Point3<ReturnScalarType> &x)
00266 {
00267     Matrix33<ReturnScalarType> C;
00268                 C[0][0]=a[0]; C[0][1]=a[1]; C[0][2]=a[2];
00269                 C[1][0]=a[1]; C[1][1]=a[3]; C[1][2]=a[4];
00270                 C[2][0]=a[2]; C[2][1]=a[4]; C[2][2]=a[5];
00271     Invert(C);
00272 
00273                 C[0][3]=-b[0]/2;
00274                 C[1][3]=-b[1]/2;
00275                 C[2][3]=-b[2]/2;
00276                 x = C * Point3<ReturnScalarType>(-b[0]/2,-b[1]/2,-b[2]/2) ;
00277     return  true;
00278 }
00279 
00280 
00281 bool MinimumNew(Point3<ScalarType> &x) const
00282 {
00283   ScalarType c0=-b[0]/2;
00284   ScalarType c1=-b[1]/2;
00285   ScalarType c2=-b[2]/2;
00286 
00287   ScalarType t125 = a[4]*a[1];
00288   ScalarType t124 = a[4]*a[4]-a[3]*a[5];
00289   ScalarType t123 = -a[1]*a[5]+a[4]*a[2];
00290   ScalarType t122 = a[2]*a[3]-t125;
00291   ScalarType t121 = -a[2]*a[1]+a[0]*a[4];
00292   ScalarType t120 = a[2]*a[2];
00293   ScalarType t119 = a[1]*a[1];
00294   ScalarType t117 = 1.0/(-a[3]*t120+2*a[2]*t125-t119*a[5]-t124*a[0]);
00295   x[0] = -(t124*c0+t122*c2-t123*c1)*t117;
00296   x[1] = (t123*c0-t121*c2+(-t120+a[0]*a[5])*c1)*t117;
00297   x[2] = -(t122*c0+(t119-a[0]*a[3])*c2+t121*c1)*t117;
00298   return true;
00299 }
00300 // determina il punto di errore minimo vincolato nel segmento (a,b)
00301 bool Minimum(Point3<ScalarType> &x,Point3<ScalarType> &pa,Point3<ScalarType> &pb){
00302 ScalarType      t1,t2, t4, t5, t8, t9,
00303         t11,t12,t14,t15,t17,t18,t25,t26,t30,t34,t35,
00304         t41,t42,t44,t45,t50,t52,t54,
00305         t56,t21,t23,t37,t64,lambda;
00306 
00307           t1 = a[4]*pb.z();
00308           t2 = t1*pa.y();
00309       t4 = a[1]*pb.y();
00310       t5 = t4*pa.x();
00311       t8 = a[1]*pa.y();
00312       t9 = t8*pa.x();
00313       t11 = a[4]*pa.z();
00314       t12 = t11*pa.y();
00315       t14 = pa.z()*pa.z();
00316       t15 = a[5]*t14;
00317       t17 = a[2]*pa.z();
00318       t18 = t17*pa.x();
00319       t21 = 2.0*t11*pb.y();
00320       t23 = a[5]*pb.z()*pa.z();
00321       t25 = a[2]*pb.z();
00322       t26 = t25*pa.x();
00323       t30 = a[0]*pb.x()*pa.x();
00324       t34 = 2.0*a[3]*pb.y()*pa.y();
00325       t35 = t17*pb.x();
00326       t37 = t8*pb.x();
00327       t41 = pa.x()*pa.x();
00328       t42 = a[0]*t41;
00329       t44 = pa.y()*pa.y();
00330       t45 = a[3]*t44;
00331       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()
00332 +(-b[0]/2)*pb.x()-(-b[1]/2)*pa.y();
00333       t52 = pb.y()*pb.y();
00334       t54 = pb.z()*pb.z();
00335       t56 = pb.x()*pb.x();
00336       t64 = t5+t37-t9+t30-t18+t35+t26-t25*pb.x()+t2-t1*pb.y()+t23;
00337       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+
00338 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()+
00339 2.0*t64)/2.0;
00340 
00341           if(lambda<0)  lambda=0;  else   if(lambda>1)   lambda = 1;
00342 
00343                  x = pa*(1.0-lambda)+pb*lambda;
00344                  return true;
00345         }
00346 
00347   void operator *= ( const ScalarType & w )                     // Amplifica una quadirca
00348         {
00349                 assert( IsValid() );
00350 
00351                 a[0] *= w;
00352                 a[1] *= w;
00353                 a[2] *= w;
00354                 a[3] *= w;
00355                 a[4] *= w;
00356                 a[5] *= w;
00357                 b[0] *= w;
00358                 b[1] *= w;
00359                 b[2] *= w;
00360                 c    *= w;
00361         }
00362 
00363 
00364 };
00365 
00366 typedef Quadric<short>  Quadrics;
00367 typedef Quadric<int>      Quadrici;
00368 typedef Quadric<float>  Quadricf;
00369 typedef Quadric<double> Quadricd;
00370 
00371 
00372 
00373         } // end namespace math
00374 } // end namespace vcg
00375 
00376 #endif