VCG Library
inertia.h
1 /****************************************************************************
2 * VCGLib o o *
3 * Visual and Computer Graphics Library o o *
4 * _ O _ *
5 * Copyright(C) 2004-2016 \/)\/ *
6 * Visual Computing Lab /\/| *
7 * ISTI - Italian National Research Council | *
8 * \ *
9 * All rights reserved. *
10 * *
11 * This program is free software; you can redistribute it and/or modify *
12 * it under the terms of the GNU General Public License as published by *
13 * the Free Software Foundation; either version 2 of the License, or *
14 * (at your option) any later version. *
15 * *
16 * This program is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
19 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
20 * for more details. *
21 * *
22 ****************************************************************************/
23 #ifndef _VCG_INERTIA_
24 #define _VCG_INERTIA_
25 
26 
27 #include <Eigen/Core>
28 #include <Eigen/Eigenvalues>
29 #include <vcg/complex/algorithms/update/normal.h>
30 
31 namespace vcg
32 {
33  namespace tri
34  {
53 template <class MeshType>
54 class Inertia
55 {
56  typedef typename MeshType::VertexType VertexType;
57  typedef typename MeshType::VertexPointer VertexPointer;
58  typedef typename MeshType::VertexIterator VertexIterator;
59  typedef typename MeshType::ScalarType ScalarType;
60  typedef typename MeshType::FaceType FaceType;
61  typedef typename MeshType::FacePointer FacePointer;
62  typedef typename MeshType::FaceIterator FaceIterator;
63  typedef typename MeshType::ConstFaceIterator ConstFaceIterator;
64  typedef typename MeshType::FaceContainer FaceContainer;
65  typedef typename MeshType::CoordType CoordType;
66 
67 private :
68  enum {X=0,Y=1,Z=2};
69  inline ScalarType SQR(ScalarType &x) const { return x*x;}
70  inline ScalarType CUBE(ScalarType &x) const { return x*x*x;}
71 
72  int A; /* alpha */
73  int B; /* beta */
74  int C; /* gamma */
75 
76 /* projection integrals */
77  double P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb;
78 
79 /* face integrals */
80  double Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca;
81 
82 /* volume integrals */
83  double T0, T1[3], T2[3], TP[3];
84 
85 public:
91  Inertia(MeshType &m) {Compute(m);}
92 
93 /* compute various integrations over projection of face */
94  void compProjectionIntegrals(FaceType &f)
95 {
96  double a0, a1, da;
97  double b0, b1, db;
98  double a0_2, a0_3, a0_4, b0_2, b0_3, b0_4;
99  double a1_2, a1_3, b1_2, b1_3;
100  double C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb;
101  double Cab, Kab, Caab, Kaab, Cabb, Kabb;
102  int i;
103 
104  P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0;
105 
106  for (i = 0; i < 3; i++) {
107  a0 = f.V(i)->P()[A];
108  b0 = f.V(i)->P()[B];
109  a1 = f.V1(i)->P()[A];
110  b1 = f.V1(i)->P()[B];
111  da = a1 - a0;
112  db = b1 - b0;
113  a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0;
114  b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0;
115  a1_2 = a1 * a1; a1_3 = a1_2 * a1;
116  b1_2 = b1 * b1; b1_3 = b1_2 * b1;
117 
118  C1 = a1 + a0;
119  Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4;
120  Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4;
121  Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2;
122  Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3;
123  Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
124  Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
125 
126  P1 += db*C1;
127  Pa += db*Ca;
128  Paa += db*Caa;
129  Paaa += db*Caaa;
130  Pb += da*Cb;
131  Pbb += da*Cbb;
132  Pbbb += da*Cbbb;
133  Pab += db*(b1*Cab + b0*Kab);
134  Paab += db*(b1*Caab + b0*Kaab);
135  Pabb += da*(a1*Cabb + a0*Kabb);
136  }
137 
138  P1 /= 2.0;
139  Pa /= 6.0;
140  Paa /= 12.0;
141  Paaa /= 20.0;
142  Pb /= -6.0;
143  Pbb /= -12.0;
144  Pbbb /= -20.0;
145  Pab /= 24.0;
146  Paab /= 60.0;
147  Pabb /= -60.0;
148 }
149 
150 
151 void CompFaceIntegrals(FaceType &f)
152 {
153  Point3<ScalarType> n;
154  ScalarType w;
155  double k1, k2, k3, k4;
156 
157  compProjectionIntegrals(f);
158 
159  n = f.N();
160  w = -f.V(0)->P()*n;
161  k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;
162 
163  Fa = k1 * Pa;
164  Fb = k1 * Pb;
165  Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
166 
167  Faa = k1 * Paa;
168  Fbb = k1 * Pbb;
169  Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb
170  + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
171 
172  Faaa = k1 * Paaa;
173  Fbbb = k1 * Pbbb;
174  Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
175  + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
176  + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
177  + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
178 
179  Faab = k1 * Paab;
180  Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
181  Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
182  + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
183 }
184 
185 
191 void Compute(MeshType &m)
192 {
194  double nx, ny, nz;
195 
196  T0 = T1[X] = T1[Y] = T1[Z]
197  = T2[X] = T2[Y] = T2[Z]
198  = TP[X] = TP[Y] = TP[Z] = 0;
199  FaceIterator fi;
200  for (fi=m.face.begin(); fi!=m.face.end();++fi) if(!(*fi).IsD() && vcg::DoubleArea(*fi)>std::numeric_limits<float>::min()) {
201  FaceType &f=(*fi);
202 
203  nx = fabs(f.N()[0]);
204  ny = fabs(f.N()[1]);
205  nz = fabs(f.N()[2]);
206  if (nx > ny && nx > nz) C = X;
207  else C = (ny > nz) ? Y : Z;
208  A = (C + 1) % 3;
209  B = (A + 1) % 3;
210 
211  CompFaceIntegrals(f);
212 
213  T0 += f.N()[X] * ((A == X) ? Fa : ((B == X) ? Fb : Fc));
214 
215  T1[A] += f.N()[A] * Faa;
216  T1[B] += f.N()[B] * Fbb;
217  T1[C] += f.N()[C] * Fcc;
218  T2[A] += f.N()[A] * Faaa;
219  T2[B] += f.N()[B] * Fbbb;
220  T2[C] += f.N()[C] * Fccc;
221  TP[A] += f.N()[A] * Faab;
222  TP[B] += f.N()[B] * Fbbc;
223  TP[C] += f.N()[C] * Fcca;
224  }
225 
226  T1[X] /= 2; T1[Y] /= 2; T1[Z] /= 2;
227  T2[X] /= 3; T2[Y] /= 3; T2[Z] /= 3;
228  TP[X] /= 2; TP[Y] /= 2; TP[Z] /= 2;
229 }
230 
235 ScalarType Mass()
236 {
237  return static_cast<ScalarType>(T0);
238 }
239 
244 Point3<ScalarType> CenterOfMass()
245 {
246  Point3<ScalarType> r;
247  r[X] = T1[X] / T0;
248  r[Y] = T1[Y] / T0;
249  r[Z] = T1[Z] / T0;
250  return r;
251 }
252 void InertiaTensor(Matrix33<ScalarType> &J ){
253  Point3<ScalarType> r;
254  r[X] = T1[X] / T0;
255  r[Y] = T1[Y] / T0;
256  r[Z] = T1[Z] / T0;
257  /* compute inertia tensor */
258  J[X][X] = (T2[Y] + T2[Z]);
259  J[Y][Y] = (T2[Z] + T2[X]);
260  J[Z][Z] = (T2[X] + T2[Y]);
261  J[X][Y] = J[Y][X] = - TP[X];
262  J[Y][Z] = J[Z][Y] = - TP[Y];
263  J[Z][X] = J[X][Z] = - TP[Z];
264 
265  J[X][X] -= T0 * (r[Y]*r[Y] + r[Z]*r[Z]);
266  J[Y][Y] -= T0 * (r[Z]*r[Z] + r[X]*r[X]);
267  J[Z][Z] -= T0 * (r[X]*r[X] + r[Y]*r[Y]);
268  J[X][Y] = J[Y][X] += T0 * r[X] * r[Y];
269  J[Y][Z] = J[Z][Y] += T0 * r[Y] * r[Z];
270  J[Z][X] = J[X][Z] += T0 * r[Z] * r[X];
271 }
272 
273 //void InertiaTensor(Matrix44<ScalarType> &J )
274 void InertiaTensor(Eigen::Matrix3d &J )
275 {
276  J=Eigen::Matrix3d::Identity();
277  Point3d r;
278  r[X] = T1[X] / T0;
279  r[Y] = T1[Y] / T0;
280  r[Z] = T1[Z] / T0;
281  /* compute inertia tensor */
282  J(X,X) = (T2[Y] + T2[Z]);
283  J(Y,Y) = (T2[Z] + T2[X]);
284  J(Z,Z) = (T2[X] + T2[Y]);
285  J(X,Y) = J(Y,X) = - TP[X];
286  J(Y,Z) = J(Z,Y) = - TP[Y];
287  J(Z,X) = J(X,Z) = - TP[Z];
288 
289  J(X,X) -= T0 * (r[Y]*r[Y] + r[Z]*r[Z]);
290  J(Y,Y) -= T0 * (r[Z]*r[Z] + r[X]*r[X]);
291  J(Z,Z) -= T0 * (r[X]*r[X] + r[Y]*r[Y]);
292  J(X,Y) = J(Y,X) += T0 * r[X] * r[Y];
293  J(Y,Z) = J(Z,Y) += T0 * r[Y] * r[Z];
294  J(Z,X) = J(X,Z) += T0 * r[Z] * r[X];
295 }
296 
297 
298 
303 void InertiaTensorEigen(Matrix33<ScalarType> &EV, Point3<ScalarType> &ev )
304 {
305  Eigen::Matrix3d it;
306  InertiaTensor(it);
307  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(it);
308  Eigen::Vector3d c_val = eig.eigenvalues();
309  Eigen::Matrix3d c_vec = eig.eigenvectors(); // eigenvector are stored as columns.
310  EV.FromEigenMatrix(c_vec);
311  EV.transposeInPlace();
312  ev.FromEigenVector(c_val);
313 }
314 
318 static void Covariance(const MeshType & m, vcg::Point3<ScalarType> & bary, vcg::Matrix33<ScalarType> &C)
319 {
320  // find the barycenter
321  ConstFaceIterator fi;
322  ScalarType area = 0.0;
323  bary.SetZero();
324  for(fi = m.face.begin(); fi != m.face.end(); ++fi)
325  if(!(*fi).IsD())
326  {
327  bary += vcg::Barycenter( *fi )* vcg::DoubleArea(*fi);
328  area+=vcg::DoubleArea(*fi);
329  }
330  bary/=area;
331 
332 
333  C.SetZero();
334  // C as covariance of triangle (0,0,0)(1,0,0)(0,1,0)
335  vcg::Matrix33<ScalarType> C0;
336  C0.SetZero();
337  C0[0][0] = C0[1][1] = 2.0;
338  C0[0][1] = C0[1][0] = 1.0;
339  C0*=1/24.0;
340 
341  // integral of (x,y,0) in the same triangle
342  CoordType X(1/6.0,1/6.0,0);
343  vcg::Matrix33<ScalarType> A, // matrix that bring the vertices to (v1-v0,v2-v0,n)
344  DC;
345  for(fi = m.face.begin(); fi != m.face.end(); ++fi)
346  if(!(*fi).IsD())
347  {
348  const CoordType &P0 = (*fi).cP(0);
349  const CoordType &P1 = (*fi).cP(1);
350  const CoordType &P2 = (*fi).cP(2);
351  CoordType n = ((P1-P0)^(P2-P0));
352  const float da = n.Norm();
353  n/=da*da;
354 
355  A.SetColumn(0, P1-P0);
356  A.SetColumn(1, P2-P0);
357  A.SetColumn(2, n);
358  CoordType delta = P0 - bary;
359 
360  /* DC is calculated as integral of (A*x+delta) * (A*x+delta)^T over the triangle,
361  where delta = v0-bary
362  */
363 
364  DC.SetZero();
365  DC+= A*C0*A.transpose();
366  vcg::Matrix33<ScalarType> tmp;
367  tmp.OuterProduct(A*X,delta);
368  DC += tmp + tmp.transpose();
369  DC+= tmp;
370  tmp.OuterProduct(delta,delta);
371  DC+=tmp*0.5;
372 // DC*=fabs(A.Determinant()); // the determinant of A is the jacobian of the change of variables A*x+delta
373  DC*=da; // the determinant of A is also the double area of *fi
374  C+=DC;
375  }
376 
377 }
378 }; // end class Inertia
379 
380  } // end namespace tri
381 } // end namespace vcg
382 
383 
384 #endif