void ComputeNodeMassTet(const Vector3d *pVertex, const int m_nVertexCount,
const int *pElement, const int nelm,
const double &rho, CSimuEntity::VertexInfo *m_pVertInfo)
{
int i;
if ((m_pVertInfo==NULL) || (m_nVertexCount <=0)){
printf("Error: object not initialized!\n");
return;
}
//First, init node mass as zero
for (i=0; i<m_nVertexCount; i++) m_pVertInfo[i].m_mass = 0;
//loop the elements;
const Vector4i *pquad = (const Vector4i *)pElement;
const double K = rho * 0.25;
for (i=0; i<nelm; i++){
const Vector4i quad = pquad[i];
const Vector3d& p0 = pVertex[quad.x];
const Vector3d& p1 = pVertex[quad.y];
const Vector3d& p2 = pVertex[quad.z];
const Vector3d& p3 = pVertex[quad.w];
const double vol = computeTetrahedronVolume(p0, p1, p2, p3);
const double mass = vol * K;
m_pVertInfo[quad.x].m_mass += mass;
m_pVertInfo[quad.y].m_mass += mass;
m_pVertInfo[quad.z].m_mass += mass;
m_pVertInfo[quad.w].m_mass += mass;
}
}
void MeshMassPropertiesTests::testTetrahedron(){
// given the four vertices of a tetrahedron verify the analytic formula for inertia
// agrees with expected results
// these numbers from the Tonon paper:
btVector3 points[4];
points[0] = btVector3(8.33220f, -11.86875f, 0.93355f);
points[1] = btVector3(0.75523f, 5.00000f, 16.37072f);
points[2] = btVector3(52.61236f, 5.00000f, -5.38580f);
points[3] = btVector3(2.00000f, 5.00000f, 3.00000f);
btScalar expectedVolume = 1873.233236f;
btMatrix3x3 expectedInertia;
expectedInertia[0][0] = 43520.33257f;
expectedInertia[1][1] = 194711.28938f;
expectedInertia[2][2] = 191168.76173f;
expectedInertia[1][2] = -4417.66150f;
expectedInertia[2][1] = -4417.66150f;
expectedInertia[0][2] = 46343.16662f;
expectedInertia[2][0] = 46343.16662f;
expectedInertia[0][1] = -11996.20119f;
expectedInertia[1][0] = -11996.20119f;
// compute volume
btScalar volume = computeTetrahedronVolume(points);
btScalar error = (volume - expectedVolume) / expectedVolume;
if (fabsf(error) > acceptableRelativeError) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR : volume of tetrahedron off by = "
<< error << std::endl;
}
btVector3 centerOfMass = 0.25f * (points[0] + points[1] + points[2] + points[3]);
// compute inertia tensor
// (shift the points so that tetrahedron's local centerOfMass is at origin)
for (int i = 0; i < 4; ++i) {
points[i] -= centerOfMass;
}
btMatrix3x3 inertia;
computeTetrahedronInertia(volume, points, inertia);
QCOMPARE_WITH_ABS_ERROR(volume, expectedVolume, acceptableRelativeError * volume);
QCOMPARE_WITH_RELATIVE_ERROR(inertia, expectedInertia, acceptableRelativeError);
}
