From 4e81e6514512c3d2e2e48515c5a25ba512515a1e Mon Sep 17 00:00:00 2001 From: ganovelli Date: Tue, 19 Aug 2008 09:43:24 +0000 Subject: [PATCH] added Covariance to to compute the covariance of a generic mesh (without the use of divergence theorem) --- vcg/complex/trimesh/inertia.h | 686 +++++++++++++++++++--------------- 1 file changed, 378 insertions(+), 308 deletions(-) diff --git a/vcg/complex/trimesh/inertia.h b/vcg/complex/trimesh/inertia.h index 9c12181b..e7cf41b4 100644 --- a/vcg/complex/trimesh/inertia.h +++ b/vcg/complex/trimesh/inertia.h @@ -1,308 +1,378 @@ -/**************************************************************************** -* VCGLib o o * -* Visual and Computer Graphics Library o o * -* _ O _ * -* Copyright(C) 2005 \/)\/ * -* Visual Computing Lab /\/| * -* ISTI - Italian National Research Council | * -* \ * -* All rights reserved. * -* * -* This program is free software; you can redistribute it and/or modify * -* it under the terms of the GNU General Public License as published by * -* the Free Software Foundation; either version 2 of the License, or * -* (at your option) any later version. * -* * -* This program is distributed in the hope that it will be useful, * -* but WITHOUT ANY WARRANTY; without even the implied warranty of * -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * -* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * -* for more details. * -* * -****************************************************************************/ -/**************************************************************************** -History - -$Log: not supported by cvs2svn $ -Revision 1.3 2006/03/29 10:12:08 corsini -Add cast to avoid warning - -Revision 1.2 2005/12/12 12:08:30 cignoni -First working version - -Revision 1.1 2005/11/21 15:58:12 cignoni -First Release (not working!) - - -Revision 1.13 2005/11/17 00:42:03 cignoni -****************************************************************************/ -/* -The algorithm is based on a three step reduction of the volume integrals -to successively simpler integrals. The algorithm is designed to minimize -the numerical errors that can result from poorly conditioned alignment of -polyhedral faces. It is also designed for efficiency. All required volume -integrals of a polyhedron are computed together during a single walk over -the boundary of the polyhedron; exploiting common subexpressions reduces -floating point operations. - -For more information, check out: - -Brian Mirtich, -``Fast and Accurate Computation of Polyhedral Mass Properties,'' -journal of graphics tools, volume 1, number 2, 1996 - -*/ - -#include -#include - -#include -namespace vcg -{ - namespace tri - { -template -class Inertia -{ - typedef InertiaMeshType MeshType; - typedef typename MeshType::VertexType VertexType; - typedef typename MeshType::VertexPointer VertexPointer; - typedef typename MeshType::VertexIterator VertexIterator; - typedef typename MeshType::ScalarType ScalarType; - typedef typename MeshType::FaceType FaceType; - typedef typename MeshType::FacePointer FacePointer; - typedef typename MeshType::FaceIterator FaceIterator; - typedef typename MeshType::FaceContainer FaceContainer; - -private : - enum {X=0,Y=1,Z=2}; - inline ScalarType SQR(ScalarType &x) const { return x*x;} - inline ScalarType CUBE(ScalarType &x) const { return x*x*x;} - - int A; /* alpha */ - int B; /* beta */ - int C; /* gamma */ - -/* projection integrals */ - double P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb; - -/* face integrals */ - double Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca; - -/* volume integrals */ - double T0, T1[3], T2[3], TP[3]; - -public: - -/* compute various integrations over projection of face */ - void compProjectionIntegrals(FaceType &f) -{ - double a0, a1, da; - double b0, b1, db; - double a0_2, a0_3, a0_4, b0_2, b0_3, b0_4; - double a1_2, a1_3, b1_2, b1_3; - double C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb; - double Cab, Kab, Caab, Kaab, Cabb, Kabb; - int i; - - P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0; - - for (i = 0; i < 3; i++) { - a0 = f.V(i)->P()[A]; - b0 = f.V(i)->P()[B]; - a1 = f.V1(i)->P()[A]; - b1 = f.V1(i)->P()[B]; - da = a1 - a0; - db = b1 - b0; - a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0; - b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0; - a1_2 = a1 * a1; a1_3 = a1_2 * a1; - b1_2 = b1 * b1; b1_3 = b1_2 * b1; - - C1 = a1 + a0; - Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4; - Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4; - Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2; - Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3; - Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3; - Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3; - - P1 += db*C1; - Pa += db*Ca; - Paa += db*Caa; - Paaa += db*Caaa; - Pb += da*Cb; - Pbb += da*Cbb; - Pbbb += da*Cbbb; - Pab += db*(b1*Cab + b0*Kab); - Paab += db*(b1*Caab + b0*Kaab); - Pabb += da*(a1*Cabb + a0*Kabb); - } - - P1 /= 2.0; - Pa /= 6.0; - Paa /= 12.0; - Paaa /= 20.0; - Pb /= -6.0; - Pbb /= -12.0; - Pbbb /= -20.0; - Pab /= 24.0; - Paab /= 60.0; - Pabb /= -60.0; -} - - -void CompFaceIntegrals(FaceType &f) -{ - Point3 n; - ScalarType w; - double k1, k2, k3, k4; - - compProjectionIntegrals(f); - - n = f.N(); - w = -f.V(0)->P()*n; - k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1; - - Fa = k1 * Pa; - Fb = k1 * Pb; - Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1); - - Faa = k1 * Paa; - Fbb = k1 * Pbb; - Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb - + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1)); - - Faaa = k1 * Paaa; - Fbbb = k1 * Pbbb; - Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab - + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb - + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb) - + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1)); - - Faab = k1 * Paab; - Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb); - Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb - + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa)); -} - - -void Compute(MeshType &m) -{ - tri::UpdateNormals::PerFaceNormalized(m); - double nx, ny, nz; - - T0 = T1[X] = T1[Y] = T1[Z] - = T2[X] = T2[Y] = T2[Z] - = TP[X] = TP[Y] = TP[Z] = 0; - FaceIterator fi; - for (fi=m.face.begin(); fi!=m.face.end();++fi) if(!(*fi).IsD()) { - FaceType &f=(*fi); - - nx = fabs(f.N()[0]); - ny = fabs(f.N()[1]); - nz = fabs(f.N()[2]); - if (nx > ny && nx > nz) C = X; - else C = (ny > nz) ? Y : Z; - A = (C + 1) % 3; - B = (A + 1) % 3; - - CompFaceIntegrals(f); - - T0 += f.N()[X] * ((A == X) ? Fa : ((B == X) ? Fb : Fc)); - - T1[A] += f.N()[A] * Faa; - T1[B] += f.N()[B] * Fbb; - T1[C] += f.N()[C] * Fcc; - T2[A] += f.N()[A] * Faaa; - T2[B] += f.N()[B] * Fbbb; - T2[C] += f.N()[C] * Fccc; - TP[A] += f.N()[A] * Faab; - TP[B] += f.N()[B] * Fbbc; - TP[C] += f.N()[C] * Fcca; - } - - T1[X] /= 2; T1[Y] /= 2; T1[Z] /= 2; - T2[X] /= 3; T2[Y] /= 3; T2[Z] /= 3; - TP[X] /= 2; TP[Y] /= 2; TP[Z] /= 2; -} - -ScalarType Mass() -{ - return static_cast(T0); -} - -Point3 CenterOfMass() -{ - Point3 r; - r[X] = T1[X] / T0; - r[Y] = T1[Y] / T0; - r[Z] = T1[Z] / T0; - return r; -} -void InertiaTensor(Matrix33 &J ){ - Point3 r; - r[X] = T1[X] / T0; - r[Y] = T1[Y] / T0; - r[Z] = T1[Z] / T0; - /* compute inertia tensor */ - J[X][X] = (T2[Y] + T2[Z]); - J[Y][Y] = (T2[Z] + T2[X]); - J[Z][Z] = (T2[X] + T2[Y]); - J[X][Y] = J[Y][X] = - TP[X]; - J[Y][Z] = J[Z][Y] = - TP[Y]; - J[Z][X] = J[X][Z] = - TP[Z]; - - J[X][X] -= T0 * (r[Y]*r[Y] + r[Z]*r[Z]); - J[Y][Y] -= T0 * (r[Z]*r[Z] + r[X]*r[X]); - J[Z][Z] -= T0 * (r[X]*r[X] + r[Y]*r[Y]); - J[X][Y] = J[Y][X] += T0 * r[X] * r[Y]; - J[Y][Z] = J[Z][Y] += T0 * r[Y] * r[Z]; - J[Z][X] = J[X][Z] += T0 * r[Z] * r[X]; -} - -void InertiaTensor(Matrix44 &J ) -{ - J.SetIdentity(); - Point3 r; - r[X] = T1[X] / T0; - r[Y] = T1[Y] / T0; - r[Z] = T1[Z] / T0; - /* compute inertia tensor */ - J[X][X] = (T2[Y] + T2[Z]); - J[Y][Y] = (T2[Z] + T2[X]); - J[Z][Z] = (T2[X] + T2[Y]); - J[X][Y] = J[Y][X] = - TP[X]; - J[Y][Z] = J[Z][Y] = - TP[Y]; - J[Z][X] = J[X][Z] = - TP[Z]; - - J[X][X] -= T0 * (r[Y]*r[Y] + r[Z]*r[Z]); - J[Y][Y] -= T0 * (r[Z]*r[Z] + r[X]*r[X]); - J[Z][Z] -= T0 * (r[X]*r[X] + r[Y]*r[Y]); - J[X][Y] = J[Y][X] += T0 * r[X] * r[Y]; - J[Y][Z] = J[Z][Y] += T0 * r[Y] * r[Z]; - J[Z][X] = J[X][Z] += T0 * r[Z] * r[X]; -} - - -// Calcola autovalori ed autovettori dell'inertia tensor. -// Gli autovettori fanno una rotmatrix che se applicata mette l'oggetto secondo gli assi id minima/max inerzia. - -void InertiaTensorEigen(Matrix44 &EV, Point4 &ev ) -{ - Matrix44 it; - InertiaTensor(it); - Matrix44d EVd,ITd;ITd.Import(it); - Point4d evd; evd.Import(ev); - int n; - Jacobi(ITd,evd,EVd,n); - EV.Import(EVd); - ev.Import(evd); -} - -}; // end class Inertia - - } // end namespace tri -} // end namespace vcg - - +/**************************************************************************** +* VCGLib o o * +* Visual and Computer Graphics Library o o * +* _ O _ * +* Copyright(C) 2005 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ +/**************************************************************************** +History + +$Log: not supported by cvs2svn $ +Revision 1.3 2006/03/29 10:12:08 corsini +Add cast to avoid warning + +Revision 1.2 2005/12/12 12:08:30 cignoni +First working version + +Revision 1.1 2005/11/21 15:58:12 cignoni +First Release (not working!) + + +Revision 1.13 2005/11/17 00:42:03 cignoni +****************************************************************************/ +#ifndef _VCG_INERTIA_ +#define _VCG_INERTIA_ + +/* +The algorithm is based on a three step reduction of the volume integrals +to successively simpler integrals. The algorithm is designed to minimize +the numerical errors that can result from poorly conditioned alignment of +polyhedral faces. It is also designed for efficiency. All required volume +integrals of a polyhedron are computed together during a single walk over +the boundary of the polyhedron; exploiting common subexpressions reduces +floating point operations. + +For more information, check out: + +Brian Mirtich, +``Fast and Accurate Computation of Polyhedral Mass Properties,'' +journal of graphics tools, volume 1, number 2, 1996 + +*/ + +#include +#include + +#include +namespace vcg +{ + namespace tri + { +template +class Inertia +{ + typedef InertiaMeshType MeshType; + typedef typename MeshType::VertexType VertexType; + typedef typename MeshType::VertexPointer VertexPointer; + typedef typename MeshType::VertexIterator VertexIterator; + typedef typename MeshType::ScalarType ScalarType; + typedef typename MeshType::FaceType FaceType; + typedef typename MeshType::FacePointer FacePointer; + typedef typename MeshType::FaceIterator FaceIterator; + typedef typename MeshType::FaceContainer FaceContainer; + typedef typename MeshType::CoordType CoordType; + typedef typename MeshType::ScalarType ScalarType; + +private : + enum {X=0,Y=1,Z=2}; + inline ScalarType SQR(ScalarType &x) const { return x*x;} + inline ScalarType CUBE(ScalarType &x) const { return x*x*x;} + + int A; /* alpha */ + int B; /* beta */ + int C; /* gamma */ + +/* projection integrals */ + double P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb; + +/* face integrals */ + double Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca; + +/* volume integrals */ + double T0, T1[3], T2[3], TP[3]; + +public: + +/* compute various integrations over projection of face */ + void compProjectionIntegrals(FaceType &f) +{ + double a0, a1, da; + double b0, b1, db; + double a0_2, a0_3, a0_4, b0_2, b0_3, b0_4; + double a1_2, a1_3, b1_2, b1_3; + double C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb; + double Cab, Kab, Caab, Kaab, Cabb, Kabb; + int i; + + P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0; + + for (i = 0; i < 3; i++) { + a0 = f.V(i)->P()[A]; + b0 = f.V(i)->P()[B]; + a1 = f.V1(i)->P()[A]; + b1 = f.V1(i)->P()[B]; + da = a1 - a0; + db = b1 - b0; + a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0; + b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0; + a1_2 = a1 * a1; a1_3 = a1_2 * a1; + b1_2 = b1 * b1; b1_3 = b1_2 * b1; + + C1 = a1 + a0; + Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4; + Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4; + Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2; + Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3; + Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3; + Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3; + + P1 += db*C1; + Pa += db*Ca; + Paa += db*Caa; + Paaa += db*Caaa; + Pb += da*Cb; + Pbb += da*Cbb; + Pbbb += da*Cbbb; + Pab += db*(b1*Cab + b0*Kab); + Paab += db*(b1*Caab + b0*Kaab); + Pabb += da*(a1*Cabb + a0*Kabb); + } + + P1 /= 2.0; + Pa /= 6.0; + Paa /= 12.0; + Paaa /= 20.0; + Pb /= -6.0; + Pbb /= -12.0; + Pbbb /= -20.0; + Pab /= 24.0; + Paab /= 60.0; + Pabb /= -60.0; +} + + +void CompFaceIntegrals(FaceType &f) +{ + Point3 n; + ScalarType w; + double k1, k2, k3, k4; + + compProjectionIntegrals(f); + + n = f.N(); + w = -f.V(0)->P()*n; + k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1; + + Fa = k1 * Pa; + Fb = k1 * Pb; + Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1); + + Faa = k1 * Paa; + Fbb = k1 * Pbb; + Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb + + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1)); + + Faaa = k1 * Paaa; + Fbbb = k1 * Pbbb; + Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab + + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb + + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb) + + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1)); + + Faab = k1 * Paab; + Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb); + Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb + + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa)); +} + + +void Compute(MeshType &m) +{ + tri::UpdateNormals::PerFaceNormalized(m); + double nx, ny, nz; + + T0 = T1[X] = T1[Y] = T1[Z] + = T2[X] = T2[Y] = T2[Z] + = TP[X] = TP[Y] = TP[Z] = 0; + FaceIterator fi; + for (fi=m.face.begin(); fi!=m.face.end();++fi) if(!(*fi).IsD()) { + FaceType &f=(*fi); + + nx = fabs(f.N()[0]); + ny = fabs(f.N()[1]); + nz = fabs(f.N()[2]); + if (nx > ny && nx > nz) C = X; + else C = (ny > nz) ? Y : Z; + A = (C + 1) % 3; + B = (A + 1) % 3; + + CompFaceIntegrals(f); + + T0 += f.N()[X] * ((A == X) ? Fa : ((B == X) ? Fb : Fc)); + + T1[A] += f.N()[A] * Faa; + T1[B] += f.N()[B] * Fbb; + T1[C] += f.N()[C] * Fcc; + T2[A] += f.N()[A] * Faaa; + T2[B] += f.N()[B] * Fbbb; + T2[C] += f.N()[C] * Fccc; + TP[A] += f.N()[A] * Faab; + TP[B] += f.N()[B] * Fbbc; + TP[C] += f.N()[C] * Fcca; + } + + T1[X] /= 2; T1[Y] /= 2; T1[Z] /= 2; + T2[X] /= 3; T2[Y] /= 3; T2[Z] /= 3; + TP[X] /= 2; TP[Y] /= 2; TP[Z] /= 2; +} + +ScalarType Mass() +{ + return static_cast(T0); +} + +Point3 CenterOfMass() +{ + Point3 r; + r[X] = T1[X] / T0; + r[Y] = T1[Y] / T0; + r[Z] = T1[Z] / T0; + return r; +} +void InertiaTensor(Matrix33 &J ){ + Point3 r; + r[X] = T1[X] / T0; + r[Y] = T1[Y] / T0; + r[Z] = T1[Z] / T0; + /* compute inertia tensor */ + J[X][X] = (T2[Y] + T2[Z]); + J[Y][Y] = (T2[Z] + T2[X]); + J[Z][Z] = (T2[X] + T2[Y]); + J[X][Y] = J[Y][X] = - TP[X]; + J[Y][Z] = J[Z][Y] = - TP[Y]; + J[Z][X] = J[X][Z] = - TP[Z]; + + J[X][X] -= T0 * (r[Y]*r[Y] + r[Z]*r[Z]); + J[Y][Y] -= T0 * (r[Z]*r[Z] + r[X]*r[X]); + J[Z][Z] -= T0 * (r[X]*r[X] + r[Y]*r[Y]); + J[X][Y] = J[Y][X] += T0 * r[X] * r[Y]; + J[Y][Z] = J[Z][Y] += T0 * r[Y] * r[Z]; + J[Z][X] = J[X][Z] += T0 * r[Z] * r[X]; +} + +void InertiaTensor(Matrix44 &J ) +{ + J.SetIdentity(); + Point3 r; + r[X] = T1[X] / T0; + r[Y] = T1[Y] / T0; + r[Z] = T1[Z] / T0; + /* compute inertia tensor */ + J[X][X] = (T2[Y] + T2[Z]); + J[Y][Y] = (T2[Z] + T2[X]); + J[Z][Z] = (T2[X] + T2[Y]); + J[X][Y] = J[Y][X] = - TP[X]; + J[Y][Z] = J[Z][Y] = - TP[Y]; + J[Z][X] = J[X][Z] = - TP[Z]; + + J[X][X] -= T0 * (r[Y]*r[Y] + r[Z]*r[Z]); + J[Y][Y] -= T0 * (r[Z]*r[Z] + r[X]*r[X]); + J[Z][Z] -= T0 * (r[X]*r[X] + r[Y]*r[Y]); + J[X][Y] = J[Y][X] += T0 * r[X] * r[Y]; + J[Y][Z] = J[Z][Y] += T0 * r[Y] * r[Z]; + J[Z][X] = J[X][Z] += T0 * r[Z] * r[X]; +} + + +/** Compute eigenvalues and eigenvectors of inertia tensor. + The eigenvectors make a rotation matrix that aligns the mesh along the axes of min/max inertia + */ +void InertiaTensorEigen(Matrix44 &EV, Point4 &ev ) +{ + Matrix44 it; + InertiaTensor(it); + Matrix44d EVd,ITd;ITd.Import(it); + Point4d evd; evd.Import(ev); + int n; + Jacobi(ITd,evd,EVd,n); + EV.Import(EVd); + ev.Import(evd); +} + +/** Compute covariance matrix of a mesh, i.e. the integral + int_{M} { (x-b)(x-b)^T }dx where b is the barycenter and x spans over the mesh M + */ +static void Covariance(MeshType m, vcg::Point3 & bary, vcg::Matrix33 &C){ + // find the barycenter + + FaceIterator fi; + ScalarType area = 0.0; + bary.Zero(); + for(fi = m.face.begin(); fi != m.face.end(); ++fi) + if(!(*fi).IsD()) + { + bary += vcg::Barycenter( *fi )* vcg::DoubleArea(*fi); + area+=vcg::DoubleArea(*fi); + } + bary/=area; + + C.SetZero(); + // C as covariance of triangle (0,0,0)(1,0,0)(0,1,0) + vcg::Matrix33 C0; + C0.SetZero(); + C0[0][0] = C0[1][1] = 2.0; + C0[0][1] = C0[1][0] = 1.0; + C0*=1/24.0; + + // integral of (x,y,0) in the same triangle + CoordType X(2/3.0,2/3.0,0); + vcg::Matrix33 A,At,DC; // matrix that bring the vertices to (v1-v0,v2-v0,n) + + for(fi = m.face.begin(); fi != m.face.end(); ++fi) + if(!(*fi).IsD()) + { + CoordType n = (*fi).N().Normalize(); + const CoordType &P0 = (*fi).P(0); + const CoordType &P1 = (*fi).P(1); + const CoordType &P2 = (*fi).P(2); + + A.SetColumn(0,P1-P0); + A.SetColumn(1,P2-P0); + A.SetColumn(2,n); + + CoordType delta = P0 - bary; + + At= A; + At.Transpose(); + + /* DC is calculated as integral of (A*x+delta) * (A*x+delta)^T over the triangle, + where delta = v0-bary + */ + + DC.SetZero(); + DC+= A*C0*At; + vcg::Matrix33 tmp; + tmp.OuterProduct(X,delta); + DC+= A * tmp; + tmp.Transpose(); + DC+= tmp * At; + tmp.OuterProduct(delta,delta); + DC+=tmp*0.5; + DC*=fabs(A.Determinant()); // the determinant of A is the jacobian of the change of variables A*x+delta + C+=DC; + } + +} +}; // end class Inertia + + } // end namespace tri +} // end namespace vcg + + +#endif \ No newline at end of file