Tighter axis-aligned bounding box for meshes.

PiperOrigin-RevId: 516166936
Change-Id: Ie99217f8894bdd4bb093900fc0df9d4a1e5030d3

src/user/user_mesh.cc

@@ -106,7 +106,8 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
   mjuu_setvec(quat_volume, 1, 0, 0, 0);
   mjuu_setvec(boxsz_surface, 0, 0, 0);
   mjuu_setvec(boxsz_volume, 0, 0, 0);
-  mjuu_setvec(aabb, 0, 0, 0);
+  mjuu_setvec(aabb, 1e10, 1e10, 1e10);
+  mjuu_setvec(aabb+3, -1e10, -1e10, -1e10);
   nvert = 0;
   nnormal = 0;
   ntexcoord = 0;
@@ -422,22 +423,10 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
     }
   }
 
-  // use AABB
+  // use aabb
   else {
-    // compute AABB
-    double AABB[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
-    for (i=0; i<nvert; i++) {
-      float* v = vert+3*i;
-      AABB[0] = mjMIN(AABB[0], v[0]);
-      AABB[1] = mjMIN(AABB[1], v[1]);
-      AABB[2] = mjMIN(AABB[2], v[2]);
-      AABB[3] = mjMAX(AABB[3], v[0]);
-      AABB[4] = mjMAX(AABB[4], v[1]);
-      AABB[5] = mjMAX(AABB[5], v[2]);
-    }
-
-    // find AABB box center
-    double cen[3] = {(AABB[0]+AABB[3])/2, (AABB[1]+AABB[4])/2, (AABB[2]+AABB[5])/2};
+    // find aabb box center
+    double cen[3] = {(aabb[0]+aabb[3])/2, (aabb[1]+aabb[4])/2, (aabb[2]+aabb[5])/2};
 
     // add box center into meshpos
     meshpos[0] += cen[0];
@@ -491,9 +480,9 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
 
     case mjGEOM_ELLIPSOID:
     case mjGEOM_BOX:
-      geom->size[0] = AABB[3] - cen[0];
-      geom->size[1] = AABB[4] - cen[1];
-      geom->size[2] = AABB[5] - cen[2];
+      geom->size[0] = aabb[3] - cen[0];
+      geom->size[1] = aabb[4] - cen[1];
+      geom->size[2] = aabb[5] - cen[2];
       break;
 
     default:
@@ -1180,6 +1169,10 @@ void mjCMesh::Process() {
         mjuu_mulvecmat(res, vec, mat);
         for (j=0; j<3; j++) {
           vert[3*i+j] = (float) res[j];
+
+          // axis-aligned bounding box
+          aabb[j+0] = mjMIN(aabb[j+0], res[j]);
+          aabb[j+3] = mjMAX(aabb[j+3], res[j]);
         }
       }
       for (i=0; i<nnormal; i++) {
@@ -1191,14 +1184,6 @@ void mjCMesh::Process() {
           normal[3*i+j] = (float) res[j];
         }
       }
-
-      // compute axis-aligned bounding box
-      for (i=0; i<nvert; i++) {
-        float* v = vert+3*i;
-        for (j=0; j<3; j++) {
-          aabb[j] = mjMAX(aabb[j], fabs(v[j]));
-        }
-      }
     }
   }
 }

src/user/user_objects.cc

@@ -1166,6 +1166,7 @@ void mjCGeom::SetInertia(void) {
 // compute radius of bounding sphere
 double mjCGeom::GetRBound(void) {
   double* aabb;
+  double haabb[3] = {0};
 
   switch (type) {
   case mjGEOM_SPHERE:
@@ -1185,7 +1186,10 @@ double mjCGeom::GetRBound(void) {
 
   case mjGEOM_MESH:
     aabb = model->meshes[meshid]->aabb;
-    return sqrt(aabb[0]*aabb[0]+aabb[1]*aabb[1]+aabb[2]*aabb[2]);
+    haabb[0] = mjMAX(fabs(aabb[0]), fabs(aabb[3]));
+    haabb[1] = mjMAX(fabs(aabb[1]), fabs(aabb[4]));
+    haabb[2] = mjMAX(fabs(aabb[2]), fabs(aabb[5]));
+    return sqrt(haabb[0]*haabb[0] + haabb[1]*haabb[1] + haabb[2]*haabb[2]);
 
   default:
     return 0;
@@ -1433,9 +1437,10 @@ void mjCGeom::Compile(void) {
     size[1] = model->hfields[hfieldid]->size[1];
     size[2] = 0.5*(model->hfields[hfieldid]->size[2]+model->hfields[hfieldid]->size[3]);
   } else if (type==mjGEOM_MESH) {
-    size[0] = model->meshes[meshid]->aabb[0];
-    size[1] = model->meshes[meshid]->aabb[1];
-    size[2] = model->meshes[meshid]->aabb[2];
+    double* aabb = model->meshes[meshid]->aabb;
+    size[0] = mjMAX(fabs(aabb[0]), fabs(aabb[3]));
+    size[1] = mjMAX(fabs(aabb[1]), fabs(aabb[4]));
+    size[2] = mjMAX(fabs(aabb[2]), fabs(aabb[5]));
   }
 
   // compute geom mass and inertia

src/user/user_objects.h

@@ -510,7 +510,7 @@ class mjCMesh: public mjCBase {
   double quat_surface[4];             // inertia orientation
   double boxsz_volume[3];             // half-sizes of equivalent inertia box (volume)
   double boxsz_surface[3];            // half-sizes of equivalent inertia box (surface)
-  double aabb[3];                     // half-sizes of axis-aligned bounding box
+  double aabb[6];                     // axis-aligned bounding box
   double volume;                      // volume of the mesh
   double surface;                     // surface of the mesh