diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index c49eee0a..6dd317e6 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -735,12 +735,12 @@ has any effect. The settings here are global and apply to the entire model. :at:`fitaabb`: :at-val:`[false, true], "false"` The compiler is able to replace a mesh with a geometric primitive fitted to that mesh; see :ref:`geom ` - below. If this attribute is "true", the fitting procedure uses the axis-aligned bounding box (aabb) of the mesh. - Otherwise it uses the equivalent-inertia box of the mesh. The type of geometric primitive used for fitting is - specified separately for each geom. The models used to generate the image on the right can be found - `here `__ (fit inertia - box) and `here `__ (fit - aabb). + below. If this attribute is "true", the fitting procedure uses the axis-aligned bounding box (AABB) of the mesh, + choosing the smallest primitive whose AABB contains the mesh AABB. Otherwise it uses the equivalent-inertia box of + the mesh. The type of geometric primitive used for fitting is specified separately for each geom. The models used to + generate the image on the right can be found `here + `__ (fit inertia box) + and `here `__ (fit aabb). .. _compiler-eulerseq: diff --git a/doc/changelog.rst b/doc/changelog.rst index a5e6ec4e..85c1b96a 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -38,6 +38,12 @@ MJX - Pull in MuJoCo Warp updatest to ``io.py``, and use ``naconmax`` instead of ``naconmax`` to set the maximum number of contacts over all environments. +Bug fixes +^^^^^^^^^ +- Fix :github:issue:`2881`, :at:`fitaabb` was adding an offset to the mesh and applying an incorrect frame + transformation. Also, unify the meaning of fitting a geom to a mesh AABB: it now means to find the smallest geom such + that its AABB contains the mesh AABB. + Version 3.3.6 (September 15, 2025) ---------------------------------- diff --git a/doc/images/changelog/meshfit.png b/doc/images/changelog/meshfit.png index 1f5e5e39..8eec2c86 100644 Binary files a/doc/images/changelog/meshfit.png and b/doc/images/changelog/meshfit.png differ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 9a69b2d6..e139b431 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -979,10 +979,7 @@ void mjCMesh::DelTexcoord() { // set geom size to match mesh -void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { - // copy mesh pos into meshpos - mjuu_copyvec(meshpos, GetPosPtr(), 3); - +void mjCMesh::FitGeom(mjCGeom* geom, double center[3]) { // use inertial box if (!model->compiler.fitaabb) { // get inertia box type (shell or volume) @@ -1016,64 +1013,35 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) { // use aamm else { - // find aabb box center - double cen[3] = {(aamm_[0]+aamm_[3])/2, (aamm_[1]+aamm_[4])/2, (aamm_[2]+aamm_[5])/2}; + // find aabb box center and size + center[0] = (aamm_[0]+aamm_[3])/2; + center[1] = (aamm_[1]+aamm_[4])/2; + center[2] = (aamm_[2]+aamm_[5])/2; + double size[3] = {aamm_[3] - center[0], aamm_[4] - center[1], aamm_[5] - center[2]}; - // add box center into meshpos - meshpos[0] += cen[0]; - meshpos[1] += cen[1]; - meshpos[2] += cen[2]; - - // compute depending on type + // compute smallest geom whose aabb contains the mesh aabb switch (geom->type) { case mjGEOM_SPHERE: - // find maximum distance - geom->size[0] = 0; - for (int i=0; i < nvert(); i++) { - double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; - double dst = mjuu_dist3(v, cen); - geom->size[0] = max(geom->size[0], dst); - } + geom->size[0] = max(max(size[0], size[1]), size[2]); break; case mjGEOM_CAPSULE: case mjGEOM_CYLINDER: // find maximum distance in XY, separately in Z - geom->size[0] = 0; - geom->size[1] = 0; - for (int i=0; i < nvert(); i++) { - double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; - double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) + - (v[1]-cen[1])*(v[1]-cen[1])); - geom->size[0] = max(geom->size[0], dst); - - // proceed with z: valid for cylinder - double dst2 = abs(v[2]-cen[2]); - geom->size[1] = max(geom->size[1], dst2); - } + geom->size[0] = max(size[0], size[1]); + geom->size[1] = size[2]; // special handling of capsule: consider curved cap if (geom->type == mjGEOM_CAPSULE) { - geom->size[1] = 0; - for (int i=0; i < nvert(); i++) { - // get distance in XY and Z - double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; - double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) + - (v[1]-cen[1])*(v[1]-cen[1])); - double dst2 = abs(v[2]-cen[2]); - - // get spherical elevation at horizontal distance dst - double h = geom->size[0] * sin(acos(dst/geom->size[0])); - geom->size[1] = max(geom->size[1], dst2-h); - } + geom->size[1] -= geom->size[0]; } break; case mjGEOM_ELLIPSOID: case mjGEOM_BOX: - geom->size[0] = aamm_[3] - cen[0]; - geom->size[1] = aamm_[4] - cen[1]; - geom->size[2] = aamm_[5] - cen[2]; + geom->size[0] = size[0]; + geom->size[1] = size[1]; + geom->size[2] = size[2]; break; default: diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 2dfdc647..421e1df5 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -3732,22 +3732,26 @@ void mjCGeom::Compile(void) { // save reference in case this is not an mjGEOM_MESH mjCMesh* pmesh = mesh; + double center[3] = {0, 0, 0}; // fit geom if type is not mjGEOM_MESH if (type != mjGEOM_MESH && type != mjGEOM_SDF) { - double meshpos[3]; - mesh->FitGeom(this, meshpos); + mesh->FitGeom(this, center); // remove reference to mesh meshname_.clear(); mesh = nullptr; - mjuu_copyvec(pmesh->GetPosPtr(), meshpos, 3); } else if (typeinertia == mjINERTIA_SHELL) { throw mjCError(this, "for mesh geoms, inertia should be specified in the mesh asset"); } - // apply geom pos/quat as offset - mjuu_frameaccum(pos, quat, pmesh->GetPosPtr(), pmesh->GetQuatPtr()); + // rotate center to geom frame and add it to mesh frame + double meshpos[3]; + mjuu_rotVecQuat(meshpos, center, pmesh->GetQuatPtr()); + mjuu_addtovec(meshpos, pmesh->GetPosPtr(), 3); + + // accumulate mesh frame into geom frame + mjuu_frameaccum(pos, quat, meshpos, pmesh->GetQuatPtr()); } // check size parameters diff --git a/src/user/user_objects.h b/src/user/user_objects.h index a92be73e..d083c404 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1186,7 +1186,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh { double* GetQuatPtr(); // get orientation double* GetInertiaBoxPtr(); // get inertia box double GetVolumeRef() const; // get volume - void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom + void FitGeom(mjCGeom* geom, double center[3]); // approximate mesh with simple geom bool HasTexcoord() const; // texcoord not null void DelTexcoord(); // delete texcoord bool IsVisual(void) const { return visual_; } // is geom visual diff --git a/test/user/testdata/fitmesh_aabb.xml b/test/user/testdata/fitmesh_aabb.xml index 7cd98c6b..7c4bca85 100644 --- a/test/user/testdata/fitmesh_aabb.xml +++ b/test/user/testdata/fitmesh_aabb.xml @@ -22,19 +22,19 @@ - + - + - + - + - +