Skip to content

Commit

Permalink
Improve mesh processing readability
Browse files Browse the repository at this point in the history 
PiperOrigin-RevId: 721524528
Change-Id: I3af4e25815cbc7517b772826af7d929ea93f8164
  • Loading branch information
@powertj @copybara-github
powertj authored and copybara-github committed Jan 30, 2025
1 parent 767be23 commit 3d9946c
Show file tree
Hide file tree
Showing 3 changed files with 110 additions and 78 deletions.
176 changes: 99 additions & 77 deletions src/user/user_mesh.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
invalidorientation_.first = -1;
invalidorientation_.second = -1;
validarea_ = true;
validvolume_ = 1;
validvolume_ = MeshVolumeOK;
valideigenvalue_ = true;
validinequality_ = true;
processed_ = false;
Expand Down Expand Up @@ -1401,10 +1401,7 @@ void mjCMesh::ComputeFaceCentroid(double facecen[3]) {


void mjCMesh::Process() {
double facecen[3] = {0, 0, 0};
double nrm[3];
double cen[3];

double facecen[3] = {0, 0, 0};;
// user offset, rotation, scaling
ApplyTransformations();

Expand All @@ -1413,7 +1410,13 @@ void mjCMesh::Process() {

double density = model->def_map[classname]->Geom().density;

bool centered = false; // true if the mesh is centered at the CoM
bool aligned_with_inertial_frame = false; // true if mesh is aligned with inertial frame

// compute inertial properties for both inertia types
// the mesh is transformed such that it is centered at the CoM and the axes
// are the principle axes of inertia. If the volume is valid, we use the volume
// inertia for this transformation, otherwise we use the shell inertia.
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};
Expand All @@ -1426,7 +1429,7 @@ void mjCMesh::Process() {
if (type == mjINERTIA_SHELL) {
validarea_ = 0;
} else {
validvolume_ = GetVolumeRef(type) < 0 ? -1 : 0;
validvolume_ = GetVolumeRef(type) < 0 ? MeshNegativeVolume : MeshZeroVolume;
}
continue;
}
Expand All @@ -1438,55 +1441,18 @@ void mjCMesh::Process() {
mjuu_copyvec(GetPosPtr(type), CoM, 3);

// re-center mesh at CoM
if (type==mjINERTIA_VOLUME || validvolume_<=0) {
// we only want to do this if the mesh is not already centered at the CoM
if (!centered) {
for (int i=0; i < nvert(); i++) {
for (int j=0; j<3; j++) {
vert_[3*i+j] -= CoM[j];
}
}
centered = true;
}

// accumulate products of inertia, recompute volume
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;
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 (inertia == mjINERTIA_LEGACY) {
vol = abs(vol);
}

// apply formula, accumulate
GetVolumeRef(type) += vol;
for (int j=0; j<6; j++) {
P[j] += density*vol /
(type==mjINERTIA_SHELL ? 12 : 20) * (
2*(D[k[j][0]] * D[k[j][1]] +
E[k[j][0]] * E[k[j][1]] +
F[k[j][0]] * F[k[j][1]]) +
D[k[j][0]] * E[k[j][1]] + D[k[j][1]] * E[k[j][0]] +
D[k[j][0]] * F[k[j][1]] + D[k[j][1]] * F[k[j][0]] +
E[k[j][0]] * F[k[j][1]] + E[k[j][1]] * F[k[j][0]]);
}
}

// convert from products of inertia to moments of inertia
inert[0] = P[1] + P[2];
inert[1] = P[0] + P[2];
inert[2] = P[0] + P[1];
inert[3] = -P[3];
inert[4] = -P[4];
inert[5] = -P[5];
// compute inertia
ComputeInertia(type, inert);

// get quaternion and diagonal inertia
double eigval[3], eigvec[9], quattmp[4];
Expand Down Expand Up @@ -1516,44 +1482,100 @@ void mjCMesh::Process() {
boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2;
boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;

// if volume was valid, copy volume quat to shell and stop,
// if mesh is aligned with inertial frame, we already successfully
// computed the volume inertia, so we can copy volume quat to shell,
// otherwise use shell quat for coordinate transformations
if (type==mjINERTIA_SHELL && validvolume_>0) {
if (aligned_with_inertial_frame) {
mjuu_copyvec(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME), 4);
continue;
}
// rotate vertices and normals to axes of inertia
// we only want to do this if the mesh is not already rotated
else {
mjuu_copyvec(GetQuatPtr(type), quattmp, 4);
Rotate(quattmp);
aligned_with_inertial_frame = true;
}
}
}

// rotate vertices and normals into axis-aligned frame
mjuu_copyvec(GetQuatPtr(type), quattmp, 4);
double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
double mat[9];
mjuu_quat2mat(mat, neg);
for (int i=0; i < nvert(); i++) {
// vertices
const double vec[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
double res[3];
mjuu_mulvecmat(res, vec, mat);
for (int j=0; j<3; j++) {
vert_[3*i+j] = (float) res[j];
void mjCMesh::ComputeInertia(mjtGeomInertia type, double inert[6]) {
double nrm[3];
double cen[3];
double density = model->def_map[classname]->Geom().density;

// axis-aligned bounding box
aamm_[j+0] = min(aamm_[j+0], res[j]);
aamm_[j+3] = max(aamm_[j+3], res[j]);
}
// accumulate products of inertia, recompute volume
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;
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 (inertia == mjINERTIA_LEGACY) {
vol = abs(vol);
}
for (int i=0; i < nnormal(); i++) {
// normals
const double nrm[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]};
double res[3];
mjuu_mulvecmat(res, nrm, mat);
for (int j=0; j<3; j++) {
normal_[3*i+j] = (float) res[j];
}

// apply formula, accumulate
GetVolumeRef(type) += vol;
for (int j=0; j<6; j++) {
P[j] += density*vol /
(type==mjINERTIA_SHELL ? 12 : 20) * (
2*(D[k[j][0]] * D[k[j][1]] +
E[k[j][0]] * E[k[j][1]] +
F[k[j][0]] * F[k[j][1]]) +
D[k[j][0]] * E[k[j][1]] + D[k[j][1]] * E[k[j][0]] +
D[k[j][0]] * F[k[j][1]] + D[k[j][1]] * F[k[j][0]] +
E[k[j][0]] * F[k[j][1]] + E[k[j][1]] * F[k[j][0]]);
}
}

// convert from products of inertia to moments of inertia
inert[0] = P[1] + P[2];
inert[1] = P[0] + P[2];
inert[2] = P[0] + P[1];
inert[3] = -P[3];
inert[4] = -P[4];
inert[5] = -P[5];
}


void mjCMesh::Rotate(double quat[4]) {
// Rotates vertices and normals of mesh by quaternion.
double neg[4] = {quat[0], -quat[1], -quat[2], -quat[3]};
double mat[9];
mjuu_quat2mat(mat, neg);
for (int i=0; i < nvert(); i++) {
// vertices
const double vec[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
double res[3];
mjuu_mulvecmat(res, vec, mat);
for (int j=0; j<3; j++) {
vert_[3*i+j] = (float) res[j];

// axis-aligned bounding box
aamm_[j+0] = min(aamm_[j+0], res[j]);
aamm_[j+3] = max(aamm_[j+3], res[j]);
}
}
for (int i=0; i < nnormal(); i++) {
// normals
const double nrm[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]};
double res[3];
mjuu_mulvecmat(res, nrm, mat);
for (int j=0; j<3; j++) {
normal_[3*i+j] = (float) res[j];
}
}
}

// check that the mesh is valid
void mjCMesh::CheckMesh(mjtGeomInertia type) {
if (!processed_) {
Expand All @@ -1566,9 +1588,9 @@ void mjCMesh::CheckMesh(mjtGeomInertia type) {
name.c_str(), invalidorientation_.first, invalidorientation_.second);
if (!validarea_ && type==mjINERTIA_SHELL)
throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
if (validvolume_<0 && type==mjINERTIA_VOLUME)
if (validvolume_==MeshNegativeVolume && type==mjINERTIA_VOLUME)
throw mjCError(this, "mesh volume is negative (misoriented triangles): %s", name.c_str());
if (!validvolume_ && type==mjINERTIA_VOLUME)
if (validvolume_==MeshZeroVolume && type==mjINERTIA_VOLUME)
throw mjCError(this, "mesh volume is too small: %s", name.c_str());
if (!valideigenvalue_)
throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
Expand Down
2 changes: 2 additions & 0 deletions src/user/user_objects.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3014,12 +3014,14 @@ void mjCGeom::Compile(void) {
meshname_.clear();
mesh = nullptr;
} else {
// Retrieve the mesh position for the relevant inertia type.
mjuu_copyvec(meshpos, mesh->GetPosPtr(typeinertia), 3);
}

// apply geom pos/quat as offset
mjuu_frameaccum(pos, quat, meshpos, pmesh->GetQuatPtr(typeinertia));
mjuu_copyvec(pmesh->GetOffsetPosPtr(), meshpos, 3);
// Retrieve the mesh quaternion for the relevant inertia type.
mjuu_copyvec(pmesh->GetOffsetQuatPtr(), pmesh->GetQuatPtr(typeinertia), 4);
}

Expand Down
10 changes: 9 additions & 1 deletion src/user/user_objects.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -863,7 +863,11 @@ class mjCMesh_ : public mjCBase {
// mesh properties that indicate a well-formed mesh
std::pair<int, int> invalidorientation_; // indices of invalid edge; -1 if none
bool validarea_; // false if the area is too small
int validvolume_; // 0: volume is too small, -1: volume is negative
enum ValidVolume {
MeshNegativeVolume = -1,
MeshZeroVolume = 0,
MeshVolumeOK = 1
} validvolume_; // indicates if volume is valid
bool valideigenvalue_; // false if inertia eigenvalue is too small
bool validinequality_; // false if inertia inequality is not satisfied
bool processed_; // false if the mesh has not been processed yet
Expand Down Expand Up @@ -993,6 +997,10 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
void ComputeFaceCentroid(double[3]); // compute centroid of all faces
void CheckMesh(mjtGeomInertia type); // check if the mesh is valid
void CopyPlugin();
void Rotate(double quat[4]); // rotate mesh by quaternion

// computes the inertia matrix of the mesh given the type of inertia
void ComputeInertia(mjtGeomInertia type, double inert[6]);

// mesh data to be copied into mjModel
double* center_; // face circumcenter data (3*nface)
Expand Down

0 comments on commit 3d9946c

Please sign in to comment.