vcglib: inertia.h Source File

00001 /****************************************************************************
00002 * VCGLib                                                            o o     *
00003 * Visual and Computer Graphics Library                            o     o   *
00004 *                                                                _   O  _   *
00005 * Copyright(C) 2005                                                \/)\/    *
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.3  2006/03/29 10:12:08  corsini
00028 Add cast to avoid warning
00029 
00030 Revision 1.2  2005/12/12 12:08:30  cignoni
00031 First working version
00032 
00033 Revision 1.1  2005/11/21 15:58:12  cignoni
00034 First Release (not working!)
00035 
00036 
00037 Revision 1.13  2005/11/17 00:42:03  cignoni
00038 ****************************************************************************/
00039 #ifndef _VCG_INERTIA_
00040 #define _VCG_INERTIA_
00041 
00042 /*
00043 The algorithm is based on a three step reduction of the volume integrals
00044 to successively simpler integrals. The algorithm is designed to minimize
00045 the numerical errors that can result from poorly conditioned alignment of
00046 polyhedral faces. It is also designed for efficiency. All required volume
00047 integrals of a polyhedron are computed together during a single walk over
00048 the boundary of the polyhedron; exploiting common subexpressions reduces
00049 floating point operations.
00050 
00051 For more information, check out:
00052 
00053 Brian Mirtich,
00054 ``Fast and Accurate Computation of Polyhedral Mass Properties,''
00055 journal of graphics tools, volume 1, number 2, 1996
00056 
00057 */
00058 
00059 #include <vcg/math/matrix33.h>
00060 #include <vcg/math/lin_algebra.h>
00061 
00062 #include <vcg/complex/trimesh/update/normal.h>
00063 namespace vcg
00064 {
00065   namespace tri
00066   {
00067 template <class InertiaMeshType>
00068 class Inertia
00069 {
00070   typedef InertiaMeshType MeshType;
00071         typedef typename MeshType::VertexType     VertexType;
00072         typedef typename MeshType::VertexPointer  VertexPointer;
00073         typedef typename MeshType::VertexIterator VertexIterator;
00074         typedef typename MeshType::ScalarType                   ScalarType;
00075         typedef typename MeshType::FaceType       FaceType;
00076         typedef typename MeshType::FacePointer    FacePointer;
00077         typedef typename MeshType::FaceIterator   FaceIterator;
00078         typedef typename MeshType::ConstFaceIterator   ConstFaceIterator;
00079         typedef typename MeshType::FaceContainer  FaceContainer;
00080         typedef typename MeshType::CoordType  CoordType;
00081 
00082 private :
00083         enum {X=0,Y=1,Z=2};
00084         inline ScalarType SQR(ScalarType &x) const { return x*x;}
00085         inline ScalarType CUBE(ScalarType &x) const { return x*x*x;}
00086 
00087  int A;   /* alpha */
00088  int B;   /* beta */
00089  int C;   /* gamma */
00090 
00091 /* projection integrals */
00092  double P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb;
00093 
00094 /* face integrals */
00095  double Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca;
00096 
00097 /* volume integrals */
00098  double T0, T1[3], T2[3], TP[3];
00099 
00100 public:
00101 
00102 /* compute various integrations over projection of face */
00103  void compProjectionIntegrals(FaceType &f)
00104 {
00105   double a0, a1, da;
00106   double b0, b1, db;
00107   double a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
00108   double a1_2, a1_3, b1_2, b1_3;
00109   double C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
00110   double Cab, Kab, Caab, Kaab, Cabb, Kabb;
00111   int i;
00112 
00113   P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;
00114 
00115   for (i = 0; i < 3; i++) {
00116     a0 = f.V(i)->P()[A];
00117     b0 = f.V(i)->P()[B];
00118     a1 = f.V1(i)->P()[A];
00119     b1 = f.V1(i)->P()[B];
00120     da = a1 - a0;
00121     db = b1 - b0;
00122     a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
00123     b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
00124     a1_2 = a1 * a1; a1_3 = a1_2 * a1;
00125     b1_2 = b1 * b1; b1_3 = b1_2 * b1;
00126 
00127     C1 = a1 + a0;
00128     Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
00129     Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
00130     Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2;
00131     Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3;
00132     Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
00133     Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
00134 
00135     P1 += db*C1;
00136     Pa += db*Ca;
00137     Paa += db*Caa;
00138     Paaa += db*Caaa;
00139     Pb += da*Cb;
00140     Pbb += da*Cbb;
00141     Pbbb += da*Cbbb;
00142     Pab += db*(b1*Cab + b0*Kab);
00143     Paab += db*(b1*Caab + b0*Kaab);
00144     Pabb += da*(a1*Cabb + a0*Kabb);
00145   }
00146 
00147   P1 /= 2.0;
00148   Pa /= 6.0;
00149   Paa /= 12.0;
00150   Paaa /= 20.0;
00151   Pb /= -6.0;
00152   Pbb /= -12.0;
00153   Pbbb /= -20.0;
00154   Pab /= 24.0;
00155   Paab /= 60.0;
00156   Pabb /= -60.0;
00157 }
00158 
00159 
00160 void CompFaceIntegrals(FaceType &f)
00161 {
00162         Point3<ScalarType>  n;
00163         ScalarType w;
00164   double k1, k2, k3, k4;
00165 
00166   compProjectionIntegrals(f);
00167 
00168   n = f.N();
00169   w = -f.V(0)->P()*n;
00170   k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;
00171 
00172   Fa = k1 * Pa;
00173   Fb = k1 * Pb;
00174   Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
00175 
00176   Faa = k1 * Paa;
00177   Fbb = k1 * Pbb;
00178   Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb
00179          + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
00180 
00181   Faaa = k1 * Paaa;
00182   Fbbb = k1 * Pbbb;
00183   Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
00184            + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
00185            + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
00186            + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
00187 
00188   Faab = k1 * Paab;
00189   Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
00190   Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
00191          + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
00192 }
00193 
00194 
00195 void Compute(MeshType &m)
00196 {
00197   tri::UpdateNormals<MeshType>::PerFaceNormalized(m);
00198   double nx, ny, nz;
00199 
00200   T0 = T1[X] = T1[Y] = T1[Z]
00201      = T2[X] = T2[Y] = T2[Z]
00202      = TP[X] = TP[Y] = TP[Z] = 0;
00203         FaceIterator fi;
00204   for (fi=m.face.begin(); fi!=m.face.end();++fi) if(!(*fi).IsD()) {
00205                 FaceType &f=(*fi);
00206 
00207     nx = fabs(f.N()[0]);
00208     ny = fabs(f.N()[1]);
00209     nz = fabs(f.N()[2]);
00210     if (nx > ny && nx > nz) C = X;
00211     else C = (ny > nz) ? Y : Z;
00212     A = (C + 1) % 3;
00213     B = (A + 1) % 3;
00214 
00215     CompFaceIntegrals(f);
00216 
00217     T0 += f.N()[X] * ((A == X) ? Fa : ((B == X) ? Fb : Fc));
00218 
00219     T1[A] += f.N()[A] * Faa;
00220     T1[B] += f.N()[B] * Fbb;
00221     T1[C] += f.N()[C] * Fcc;
00222     T2[A] += f.N()[A] * Faaa;
00223     T2[B] += f.N()[B] * Fbbb;
00224     T2[C] += f.N()[C] * Fccc;
00225     TP[A] += f.N()[A] * Faab;
00226     TP[B] += f.N()[B] * Fbbc;
00227     TP[C] += f.N()[C] * Fcca;
00228   }
00229 
00230   T1[X] /= 2; T1[Y] /= 2; T1[Z] /= 2;
00231   T2[X] /= 3; T2[Y] /= 3; T2[Z] /= 3;
00232   TP[X] /= 2; TP[Y] /= 2; TP[Z] /= 2;
00233 }
00234 
00235 ScalarType Mass()
00236 {
00237         return static_cast<ScalarType>(T0);
00238 }
00239 
00240 Point3<ScalarType>  CenterOfMass()
00241 {
00242         Point3<ScalarType>  r;
00243   r[X] = T1[X] / T0;
00244   r[Y] = T1[Y] / T0;
00245   r[Z] = T1[Z] / T0;
00246         return r;
00247 }
00248 void InertiaTensor(Matrix33<ScalarType> &J ){
00249         Point3<ScalarType>  r;
00250   r[X] = T1[X] / T0;
00251   r[Y] = T1[Y] / T0;
00252   r[Z] = T1[Z] / T0;
00253   /* compute inertia tensor */
00254   J[X][X] = (T2[Y] + T2[Z]);
00255   J[Y][Y] = (T2[Z] + T2[X]);
00256   J[Z][Z] = (T2[X] + T2[Y]);
00257   J[X][Y] = J[Y][X] = - TP[X];
00258   J[Y][Z] = J[Z][Y] = - TP[Y];
00259   J[Z][X] = J[X][Z] = - TP[Z];
00260 
00261   J[X][X] -= T0 * (r[Y]*r[Y] + r[Z]*r[Z]);
00262   J[Y][Y] -= T0 * (r[Z]*r[Z] + r[X]*r[X]);
00263   J[Z][Z] -= T0 * (r[X]*r[X] + r[Y]*r[Y]);
00264   J[X][Y] = J[Y][X] += T0 * r[X] * r[Y];
00265   J[Y][Z] = J[Z][Y] += T0 * r[Y] * r[Z];
00266   J[Z][X] = J[X][Z] += T0 * r[Z] * r[X];
00267 }
00268 
00269 void InertiaTensor(Matrix44<ScalarType> &J )
00270 {
00271         J.SetIdentity();
00272         Point3<ScalarType>  r;
00273   r[X] = T1[X] / T0;
00274   r[Y] = T1[Y] / T0;
00275   r[Z] = T1[Z] / T0;
00276   /* compute inertia tensor */
00277   J[X][X] = (T2[Y] + T2[Z]);
00278   J[Y][Y] = (T2[Z] + T2[X]);
00279   J[Z][Z] = (T2[X] + T2[Y]);
00280   J[X][Y] = J[Y][X] = - TP[X];
00281   J[Y][Z] = J[Z][Y] = - TP[Y];
00282   J[Z][X] = J[X][Z] = - TP[Z];
00283 
00284   J[X][X] -= T0 * (r[Y]*r[Y] + r[Z]*r[Z]);
00285   J[Y][Y] -= T0 * (r[Z]*r[Z] + r[X]*r[X]);
00286   J[Z][Z] -= T0 * (r[X]*r[X] + r[Y]*r[Y]);
00287   J[X][Y] = J[Y][X] += T0 * r[X] * r[Y];
00288   J[Y][Z] = J[Z][Y] += T0 * r[Y] * r[Z];
00289   J[Z][X] = J[X][Z] += T0 * r[Z] * r[X];
00290 }
00291 
00292 
00296 void InertiaTensorEigen(Matrix44<ScalarType> &EV, Point4<ScalarType> &ev )
00297 {
00298         Matrix44<ScalarType> it;
00299         InertiaTensor(it);
00300         Matrix44d EVd,ITd;ITd.Import(it);
00301   Point4d evd;
00302         int n;
00303         Jacobi(ITd,evd,EVd,n);
00304         EV.Import(EVd);
00305         ev.Import(evd);
00306 }
00307 
00311 static void Covariance(const MeshType & m, vcg::Point3<ScalarType> & bary, vcg::Matrix33<ScalarType> &C){
00312         // find the barycenter
00313 
00314         ConstFaceIterator fi;
00315         ScalarType area = 0.0;
00316         bary.SetZero();
00317         for(fi = m.face.begin(); fi != m.face.end(); ++fi)
00318                 if(!(*fi).IsD())
00319                         {
00320                                 bary += vcg::Barycenter( *fi )* vcg::DoubleArea(*fi);
00321                                 area+=vcg::DoubleArea(*fi);
00322                         }
00323         bary/=area;
00324 
00325 
00326         C.SetZero();
00327         // C as covariance of triangle (0,0,0)(1,0,0)(0,1,0)
00328         vcg::Matrix33<ScalarType> C0;
00329         C0.SetZero();
00330         C0[0][0] = C0[1][1] = 2.0;
00331         C0[0][1] = C0[1][0] = 1.0;
00332         C0*=1/24.0;
00333 
00334         // integral of (x,y,0) in the same triangle
00335         CoordType X(1/6.0,1/6.0,0);
00336         vcg::Matrix33<ScalarType> A, // matrix that bring the vertices to (v1-v0,v2-v0,n)
00337                                                                                                     DC;
00338         for(fi = m.face.begin(); fi != m.face.end(); ++fi)
00339                 if(!(*fi).IsD())
00340                 {
00341                         const CoordType &P0 = (*fi).P(0);
00342                         const CoordType &P1 = (*fi).P(1);
00343                         const CoordType &P2 = (*fi).P(2);
00344                         CoordType  n = ((P1-P0)^(P2-P0));
00345                         const float da = n.Norm();
00346                         n/=da*da;
00347 
00348                         #ifndef VCG_USE_EIGEN
00349                         A.SetColumn(0, P1-P0);
00350                         A.SetColumn(1, P2-P0);
00351                         A.SetColumn(2, n);
00352                         #else
00353                         A.col(0) = P1-P0;
00354                         A.col(1) = P2-P0;
00355                         A.col(2) = n;
00356                         #endif
00357                         CoordType delta = P0 - bary;
00358 
00359                         /* DC is calculated as integral of (A*x+delta) * (A*x+delta)^T over the triangle,
00360                                  where delta = v0-bary
00361                         */
00362 
00363                         DC.SetZero();
00364                         DC+= A*C0*A.transpose();
00365                         vcg::Matrix33<ScalarType> tmp;
00366                         tmp.OuterProduct(A*X,delta);
00367                         DC += tmp + tmp.transpose();
00368                         DC+= tmp;
00369                         tmp.OuterProduct(delta,delta);
00370                         DC+=tmp*0.5;
00371 //              DC*=fabs(A.Determinant());      // the determinant of A is the jacobian of the change of variables A*x+delta
00372                         DC*=da;                                                                                 // the determinant of A is also the double area of *fi
00373                         C+=DC;
00374         }
00375 
00376 }
00377 }; // end class Inertia
00378 
00379   } // end namespace tri
00380 } // end namespace vcg
00381 
00382 
00383 #endif