VCG Library
Loading...
Searching...
No Matches
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
31namespace vcg
32{
33 namespace tri
34 {
53template <class MeshType>
54class 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
67private :
68 enum {X=0,Y=1,Z=2};
69 inline ScalarType SQR(const ScalarType &x) const { return x*x;}
70 inline ScalarType CUBE(const 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
85public:
91 Inertia(const MeshType &m) { Compute(m); }
92
93/* compute various integrations over projection of face */
94 void compProjectionIntegrals(const 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
151void CompFaceIntegrals(const FaceType &f, const Point3<ScalarType> &n)
152{
153 ScalarType w;
154 double k1, k2, k3, k4;
155
156 compProjectionIntegrals(f);
157
158 w = -f.V(0)->P()*n;
159 k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1;
160
161 Fa = k1 * Pa;
162 Fb = k1 * Pb;
163 Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
164
165 Faa = k1 * Paa;
166 Fbb = k1 * Pbb;
167 Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb
168 + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
169
170 Faaa = k1 * Paaa;
171 Fbbb = k1 * Pbbb;
172 Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
173 + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
174 + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
175 + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
176
177 Faab = k1 * Paab;
178 Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
179 Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
180 + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
181}
182
183
189void Compute(const MeshType &m)
190{
191 double nx, ny, nz;
192
193 T0 = T1[X] = T1[Y] = T1[Z]
194 = T2[X] = T2[Y] = T2[Z]
195 = TP[X] = TP[Y] = TP[Z] = 0;
196 for (auto fi=m.face.begin(); fi!=m.face.end();++fi) if(!(*fi).IsD() && vcg::DoubleArea(*fi)>std::numeric_limits<float>::min())
197 {
198 const FaceType &f=(*fi);
199 const auto fn = vcg::NormalizedTriangleNormal(f);
200
201 nx = fabs(fn[0]);
202 ny = fabs(fn[1]);
203 nz = fabs(fn[2]);
204 if (nx > ny && nx > nz) C = X;
205 else C = (ny > nz) ? Y : Z;
206 A = (C + 1) % 3;
207 B = (A + 1) % 3;
208
209 CompFaceIntegrals(f, fn);
210
211 T0 += fn[X] * ((A == X) ? Fa : ((B == X) ? Fb : Fc));
212
213 T1[A] += fn[A] * Faa;
214 T1[B] += fn[B] * Fbb;
215 T1[C] += fn[C] * Fcc;
216 T2[A] += fn[A] * Faaa;
217 T2[B] += fn[B] * Fbbb;
218 T2[C] += fn[C] * Fccc;
219 TP[A] += fn[A] * Faab;
220 TP[B] += fn[B] * Fbbc;
221 TP[C] += fn[C] * Fcca;
222 }
223
224 T1[X] /= 2; T1[Y] /= 2; T1[Z] /= 2;
225 T2[X] /= 3; T2[Y] /= 3; T2[Z] /= 3;
226 TP[X] /= 2; TP[Y] /= 2; TP[Z] /= 2;
227}
228
233ScalarType Mass(void) const
234{
235 return static_cast<ScalarType>(T0);
236}
237
242Point3<ScalarType> CenterOfMass(void) const
243{
244 Point3<ScalarType> r;
245 r[X] = T1[X] / T0;
246 r[Y] = T1[Y] / T0;
247 r[Z] = T1[Z] / T0;
248 return r;
249}
250
251void InertiaTensor(Matrix33<ScalarType> &J) const
252{
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 )
274void InertiaTensor(Eigen::Matrix3d &J) const
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
303void InertiaTensorEigen(Matrix33<ScalarType> &EV, Point3<ScalarType> &ev) const
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
318static 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
vcg::Point3
Definition: point3.h:43
vcg::tri::Inertia
Methods for computing Polyhedral Mass properties (like inertia tensor, volume, etc)
Definition: inertia.h:55
vcg::tri::Inertia::Inertia
Inertia(const MeshType &m)
Basic constructor.
Definition: inertia.h:91
vcg::tri::Inertia::CenterOfMass
Point3< ScalarType > CenterOfMass(void) const
Return the Center of Mass (or barycenter) of the mesh.
Definition: inertia.h:242
vcg::tri::Inertia::InertiaTensorEigen
void InertiaTensorEigen(Matrix33< ScalarType > &EV, Point3< ScalarType > &ev) const
Return the Inertia tensor the mesh.
Definition: inertia.h:303
vcg::tri::Inertia::Compute
void Compute(const MeshType &m)
Definition: inertia.h:189
vcg::tri::Inertia::Covariance
static void Covariance(const MeshType &m, vcg::Point3< ScalarType > &bary, vcg::Matrix33< ScalarType > &C)
Definition: inertia.h:318
vcg::tri::Inertia::Mass
ScalarType Mass(void) const
Return the Volume (or mass) of the mesh.
Definition: inertia.h:233
vcg
Definition: color4.h:30
Generated by doxygen 1.9.6 on Mon Dec 1 2025 17:19:44