added Covariance to to compute the covariance of a generic mesh (without the use of divergence theorem)
This commit is contained in:
ganovelli
parent
1ac5c66e78
commit
4e81e65145
|
|
@ -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 <vcg/math/matrix33.h>
|
||||
#include <vcg/math/lin_algebra.h>
|
||||
|
||||
#include <vcg/complex/trimesh/update/normal.h>
|
||||
namespace vcg
|
||||
{
|
||||
namespace tri
|
||||
{
|
||||
template <class InertiaMeshType>
|
||||
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<ScalarType> 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<MeshType>::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<ScalarType>(T0);
|
||||
}
|
||||
|
||||
Point3<ScalarType> CenterOfMass()
|
||||
{
|
||||
Point3<ScalarType> r;
|
||||
r[X] = T1[X] / T0;
|
||||
r[Y] = T1[Y] / T0;
|
||||
r[Z] = T1[Z] / T0;
|
||||
return r;
|
||||
}
|
||||
void InertiaTensor(Matrix33<ScalarType> &J ){
|
||||
Point3<ScalarType> 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<ScalarType> &J )
|
||||
{
|
||||
J.SetIdentity();
|
||||
Point3<ScalarType> 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<ScalarType> &EV, Point4<ScalarType> &ev )
|
||||
{
|
||||
Matrix44<ScalarType> 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 <vcg/math/matrix33.h>
|
||||
#include <vcg/math/lin_algebra.h>
|
||||
|
||||
#include <vcg/complex/trimesh/update/normal.h>
|
||||
namespace vcg
|
||||
{
|
||||
namespace tri
|
||||
{
|
||||
template <class InertiaMeshType>
|
||||
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<ScalarType> 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<MeshType>::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<ScalarType>(T0);
|
||||
}
|
||||
|
||||
Point3<ScalarType> CenterOfMass()
|
||||
{
|
||||
Point3<ScalarType> r;
|
||||
r[X] = T1[X] / T0;
|
||||
r[Y] = T1[Y] / T0;
|
||||
r[Z] = T1[Z] / T0;
|
||||
return r;
|
||||
}
|
||||
void InertiaTensor(Matrix33<ScalarType> &J ){
|
||||
Point3<ScalarType> 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<ScalarType> &J )
|
||||
{
|
||||
J.SetIdentity();
|
||||
Point3<ScalarType> 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<ScalarType> &EV, Point4<ScalarType> &ev )
|
||||
{
|
||||
Matrix44<ScalarType> 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<ScalarType> & bary, vcg::Matrix33<ScalarType> &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<ScalarType> 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<ScalarType> 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<ScalarType> 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
|
||||
Loading…
Reference in New Issue