Move mesh inertia specification from global <compiler> to <asset/mesh>.

Three options:
- Convex: Use convex hull
- Exact: Use exact geometry (requires watertight mesh)
- Legacy: Legacy approximation (deprecated)

Fixes #2152

PiperOrigin-RevId: 688949900
Change-Id: I4537f343db917195c6fd58d517b189d5ee3be1cd
This commit is contained in:
Alessio Quaglino
2024-10-23 07:05:32 -07:00
committed by Copybara-Service
parent 0c97dcf1fd
commit b598d79b3f
19 changed files with 170 additions and 87 deletions
+1
View File
@@ -246,6 +246,7 @@ void mjs_defaultMesh(mjsMesh* mesh) {
mesh->refquat[0] = 1;
mesh->scale[0] = mesh->scale[1] = mesh->scale[2] = 1;
mesh->maxhullvert = -1;
mesh->inertia = mjINERTIA_LEGACY;
}
+22 -22
View File
@@ -1250,22 +1250,24 @@ void mjCMesh::LoadMSH(mjResource* resource) {
void mjCMesh::ComputeVolume(double CoM[3], mjtGeomInertia type,
const double facecen[3], bool exactmeshinertia) {
const double facecen[3]) {
double nrm[3];
double cen[3];
GetVolumeRef(type) = 0;
mjuu_zerovec(CoM, 3);
for (int i=0; i < nface(); i++) {
int nf = (inertia == mjINERTIA_CONVEX) ? graph_[1] : nface();
int* f = (inertia == mjINERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data();
float* vv = vert_.data();
for (int i=0; i < nf; i++) {
// get area, normal and center
float* vv = vert_.data();
double a = _triangle(nrm, cen, vv+3*face_[3*i], vv+3*face_[3*i+1], vv+3*face_[3*i+2]);
double a = _triangle(nrm, cen, vv+3*f[3*i], vv+3*f[3*i+1], vv+3*f[3*i+2]);
// compute and add volume
const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]};
double vol = type==mjINERTIA_SHELL ? a : mjuu_dot3(vec, nrm) * a / 3;
// if legacy computation requested, then always positive
if (!exactmeshinertia && type==mjINERTIA_VOLUME) {
if (inertia == mjINERTIA_LEGACY) {
vol = abs(vol);
}
@@ -1410,21 +1412,17 @@ void mjCMesh::Process() {
for ( const auto type : { mjtGeomInertia::mjINERTIA_VOLUME, mjtGeomInertia::mjINERTIA_SHELL } ) {
double CoM[3] = {0, 0, 0};
double inert[6] = {0, 0, 0, 0, 0, 0};
bool exactmeshinertia = model->exactmeshinertia;
// compute CoM and volume from pyramid volumes
ComputeVolume(CoM, type, facecen, model->exactmeshinertia);
ComputeVolume(CoM, type, facecen);
// perform computation with convex mesh if volume is negative
if (GetVolumeRef(type) <= 0 && exactmeshinertia) {
mju_warning("Malformed mesh '%s', computing mesh inertia from convex hull", name.c_str());
exactmeshinertia = false;
ComputeVolume(CoM, type, facecen, exactmeshinertia);
}
// if volume is still invalid, skip the rest of the computations
// if volume is invalid, skip the rest of the computations
if (GetVolumeRef(type) < mjMINVAL) {
validvolume_ = GetVolumeRef(type) < 0 ? -1 : 0;
if (type == mjINERTIA_SHELL) {
validarea_ = 0;
} else {
validvolume_ = GetVolumeRef(type) < 0 ? -1 : 0;
}
continue;
}
@@ -1447,17 +1445,19 @@ void mjCMesh::Process() {
const int k[6][2] = {{0, 0}, {1, 1}, {2, 2}, {0, 1}, {0, 2}, {1, 2}};
double P[6] = {0, 0, 0, 0, 0, 0};
GetVolumeRef(type) = 0;
for (int i=0; i < nface(); i++) {
float* D = vert_.data()+3*face_[3*i];
float* E = vert_.data()+3*face_[3*i+1];
float* F = vert_.data()+3*face_[3*i+2];
int nf = (inertia == mjINERTIA_CONVEX) ? graph_[1] : nface();
int* f = (inertia == mjINERTIA_CONVEX) ? graph_ + 2 + 3*(graph_[0]+graph_[1]) : face_.data();
for (int i=0; i < nf; i++) {
float* D = vert_.data()+3*f[3*i];
float* E = vert_.data()+3*f[3*i+1];
float* F = vert_.data()+3*f[3*i+2];
// get area, normal and center; update volume
double a = _triangle(nrm, cen, D, E, F);
double vol = type==mjINERTIA_SHELL ? a : mjuu_dot3(cen, nrm) * a / 3;
// if legacy computation requested, then always positive
if (!exactmeshinertia && type==mjINERTIA_VOLUME) {
if (inertia == mjINERTIA_LEGACY) {
vol = abs(vol);
}
@@ -1554,7 +1554,7 @@ void mjCMesh::CheckMesh(mjtGeomInertia type) {
if (!processed_) {
return;
}
if ((invalidorientation_.first>=0 || invalidorientation_.second>=0) && model->exactmeshinertia)
if ((invalidorientation_.first>=0 || invalidorientation_.second>=0) && inertia == mjINERTIA_EXACT)
throw mjCError(this,
"faces of mesh '%s' have inconsistent orientation. Please check the "
"faces containing the vertices %d and %d.",
+2 -2
View File
@@ -868,6 +868,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
const std::vector<float>& UserTexcoord() const { return spec_texcoord_; }
const std::vector<int>& Face() const { return face_; }
const std::vector<int>& UserFace() const { return spec_face_; }
mjtMeshInertia Inertia() const { return spec.inertia; }
// setters
void SetNeedHull(bool needhull) { needhull_ = needhull; }
@@ -945,8 +946,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
std::vector<unsigned char> num_face_vertices_;
// compute the volume and center-of-mass of the mesh given the face center
void ComputeVolume(double CoM[3], mjtGeomInertia type, const double facecen[3],
bool exactmeshinertia);
void ComputeVolume(double CoM[3], mjtGeomInertia gtype, const double facecen[3]);
};