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:
committed by
Copybara-Service
parent
0c97dcf1fd
commit
b598d79b3f
@@ -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
@@ -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.",
|
||||
|
||||
@@ -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]);
|
||||
};
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user