//
//    Polyhedral Mass Properties
//        - Volume
//        - Center Of Mass
//        - Inertial Tensor
//    by: S Melax
//
//
// Routines for calculating physical properties of a mesh.
// Input vertices and index triangle lists. 
// The interfaces were designed so you could steal these functions 
// and incorporate into your own environment extremely quickly.
//
// 
// The Inertial Tensor routine Inertia() does its summation 
// with respect to the center of mass which it assumes is [0,0,0] if
// none is provided.  
//
// Note, There are cases where you actually want the inertial properties 
// from a point other than the center of mass.  In particular, when 
// combining (possibly interpenetrating) geometries with different 
// masses and mass densities into a compound rigid body.  
// Inertia tensors can be rotated and translated after the fact, but
// the operation is different than the usual matrix transforms. 
// 
// From a triangle with vertices v1,v2 and v3
// the 4 vertices of the tetrahedron are v0,v1,v2,v3
// where v0=[0 0 0].
// These algorithems are based on tetrahedral summation.
// See http://www.melax.com/volint/  for more discussion.
//
// 




float Volume(const float3 *vertices, const int3 *tris, const int count) 
{
    // count is the number of triangles (tris) 
    float  volume=0;
    for(int i=0; i<count; i++)  // for each triangle
    {
        volume += Determinant(float3x3(vertices[tris[i][0]],vertices[tris[i][1]],vertices[tris[i][2]])); //divide by 6 later for efficiency
    }
    return volume/6.0f;  // since the determinant give 6 times tetra volume
}
 
float3 CenterOfMass(const float3 *vertices, const int3 *tris, const int count) 
{
	// count is the number of triangles (tris) 
	float3 com(0,0,0);
	float  volume=0; // actually accumulates the volume*6
	for(int i=0; i<count; i++)  // for each triangle
	{
		float3x3 A(vertices[tris[i][0]],vertices[tris[i][1]],vertices[tris[i][2]]);  
		float vol=Determinant(A);  // dont bother to divide by 6 
		com += vol * (A.x+A.y+A.z);  // divide by 4 at end
		volume+=vol;
	}
	com /= volume*4.0f; 
	return com;
}

float3x3 Inertia(const float3 *vertices, const int3 *tris, const int count, const float3& com=float3(0,0,0)) 
{
	// count is the number of triangles (tris) 
	// The moments are calculated based on the center of rotation (com) which defaults to [0,0,0] if unsupplied
	// assume mass==1.0  you can multiply by mass later.
	// for improved accuracy the next 3 variables, the determinant d, and its calculation should be changed to double
	float  volume=0;                          // technically this variable accumulates the volume times 6
	float3 diag(0,0,0);                       // accumulate matrix main diagonal integrals [x*x, y*y, z*z]
	float3 offd(0,0,0);                       // accumulate matrix off-diagonal  integrals [y*z, x*z, x*y]
	for(int i=0; i<count; i++)  // for each triangle
	{
		float3x3 A(vertices[tris[i][0]]-com,vertices[tris[i][1]]-com,vertices[tris[i][2]]-com);  // matrix trick for volume calc by taking determinant
		float    d = Determinant(A);  // vol of tiny parallelapiped= d * dr * ds * dt (the 3 partials of my tetral triple integral equasion)
		volume +=d;                   // add vol of current tetra (note it could be negative - that's ok we need that sometimes)
		for(int j=0;j<3;j++)
		{
			int j1=(j+1)%3;   
			int j2=(j+2)%3;   
			diag[j] += (A[0][j]*A[1][j] + A[1][j]*A[2][j] + A[2][j]*A[0][j] + 
						A[0][j]*A[0][j] + A[1][j]*A[1][j] + A[2][j]*A[2][j]  ) *d; // divide by 60.0f later;
			offd[j] += (A[0][j1]*A[1][j2]  + A[1][j1]*A[2][j2]  + A[2][j1]*A[0][j2]  +
						A[0][j1]*A[2][j2]  + A[1][j1]*A[0][j2]  + A[2][j1]*A[1][j2]  +
						A[0][j1]*A[0][j2]*2+ A[1][j1]*A[1][j2]*2+ A[2][j1]*A[2][j2]*2 ) *d; // divide by 120.0f later
		}
	}
	diag /= volume*(60.0f /6.0f);  // divide by total volume (vol/6) since density=1/volume
	offd /= volume*(120.0f/6.0f);
	return float3x3(diag.y+diag.z  , -offd.z      , -offd.y,
				   -offd.z        , diag.x+diag.z, -offd.x,
				   -offd.y        , -offd.x      , diag.x+diag.y );
}