diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 0a3038c5..9c4973dd 100644 --- a/src/user/user_mesh.cc +++ b/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; isize[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; imeshes[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 diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 9e2fd391..80bf7189 100644 --- a/src/user/user_objects.h +++ b/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