vcglib/vcg/complex/trimesh/inertia.h

/****************************************************************************
* 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