diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index a4d62eb7..53ec258c 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -480,24 +480,6 @@ static void mapPose(const mjtNum xpos1[3], const mjtNum xquat1[4], mju_quat2Mat(mat12, quat12); } -// subtract mesh position from sdf transformation -static void undoTransformation(const mjModel* m, const mjData* d, int g, - mjtNum sdf_xpos[3], mjtNum sdf_quat[4]) { - mjtNum* xpos = d->geom_xpos + 3 * g; - mjtNum* xmat = d->geom_xmat + 9 * g; - if (m->geom_type[g] == mjGEOM_MESH || m->geom_type[g] == mjGEOM_SDF) { - mjtNum negpos[3], negquat[4], xquat[4]; - mjtNum* pos = m->mesh_pos + 3 * m->geom_dataid[g]; - mjtNum* quat = m->mesh_quat + 4 * m->geom_dataid[g]; - mju_mat2Quat(xquat, xmat); - mju_negPose(negpos, negquat, pos, quat); - mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat); - } else { - mju_copy3(sdf_xpos, xpos); - mju_mat2Quat(sdf_quat, xmat); - } -} - //---------------------------- narrow phase ----------------------------------------------- // comparison function for contact sorting @@ -767,8 +749,7 @@ int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int // collision between a mesh and a signed distance field int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { - mjtNum* pos1 = d->geom_xpos + 3 * g1; - mjtNum* mat1 = d->geom_xmat + 9 * g1; + mjGETINFO; mjtNum offset[3], rotation[9], corners[9], x[3], depth; mjtNum points[3*MAXSDFFACE], dist[MAXMESHPNT], candidate[3*MAXMESHPNT]; @@ -790,10 +771,10 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g sdf.geomtype = &geomtype; // compute transformation from g1 to g2 - mjtNum pos2true[3], sdf_quat[4], quat1[4]; + mjtNum sdf_quat[4], quat1[4]; mju_mat2Quat(quat1, mat1); - undoTransformation(m, d, g2, pos2true, sdf_quat); - mapPose(pos1, quat1, pos2true, sdf_quat, offset, rotation); + mju_mat2Quat(sdf_quat, mat2); + mapPose(pos1, quat1, pos2, sdf_quat, offset, rotation); // binary tree search collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf); @@ -845,7 +826,7 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g // add only the first mjMAXCONPAIR pairs for (int i=0; i < mju_min(ncandidate, mjMAXCONPAIR); i++) { - cnt = addContact(points, con, candidate + 3*index[i], pos2true, sdf_quat, + cnt = addContact(points, con, candidate + 3*index[i], pos2, sdf_quat, dist[index[i]], cnt, m, &sdf, (mjData*)d); } @@ -871,17 +852,13 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m // compute transformations from/to g1 to/from g2 mjtNum quat1[4], quat2[4]; - mjtNum pos1true[3], offset21[3], rotation21[9], rotation12[9]; - mjtNum pos2true[3], offset1[3], rotation1[9], offset12[3]; - mjtNum offset2[3], rotation2[9], squat1[4], squat2[4]; - undoTransformation(m, d, g1, pos1true, squat1); - undoTransformation(m, d, g2, pos2true, squat2); + mjtNum offset21[3], rotation21[9], rotation12[9]; + mjtNum offset12[3], offset2[3], rotation2[9]; mju_mat2Quat(quat1, mat1); mju_mat2Quat(quat2, mat2); - mapPose(pos2, quat2, pos1, quat1, offset1, rotation1); - mapPose(pos1, quat1, pos1true, squat1, offset2, rotation2); - mapPose(pos2true, squat2, pos1true, squat1, offset21, rotation21); - mapPose(pos1true, squat1, pos2true, squat2, offset12, rotation12); + mapPose(pos1, quat1, pos1, quat1, offset2, rotation2); + mapPose(pos2, quat2, pos1, quat1, offset21, rotation21); + mapPose(pos1, quat1, pos2, quat2, offset12, rotation12); // axis-aligned bounding boxes in g1 frame for (int i=0; i < 8; i++) { @@ -893,8 +870,8 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m vec2[1] = (i&2 ? size2[1]+size2[4] : size2[1]-size2[4]); vec2[2] = (i&4 ? size2[2]+size2[5] : size2[2]-size2[5]); - mju_mulMatVec3(vec2, rotation1, vec2); - mju_addTo3(vec2, offset1); + mju_mulMatVec3(vec2, rotation21, vec2); + mju_addTo3(vec2, offset21); for (int k=0; k < 3; k++) { aabb1[0+k] = mju_min(aabb1[0+k], vec1[k]); @@ -983,7 +960,7 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m // contact point and normal - we use the midsurface where SDF1=SDF2 as zero level set sdf.type = mjSDFTYPE_MIDSURFACE; - cnt = addContact(contacts, con, x, pos2true, squat2, dist, cnt, m, &sdf, (mjData*)d); + cnt = addContact(contacts, con, x, pos2, quat2, dist, cnt, m, &sdf, (mjData*)d); // SHOULD NOT OCCUR if (cnt > mjMAXCONPAIR) { diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index db58e69c..d3bd8621 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -22,6 +22,7 @@ #include #include // IWYU pragma: keep #include +#include "engine/engine_collision_sdf.h" #include "engine/engine_io.h" #include "engine/engine_plugin.h" #include "engine/engine_util_blas.h" @@ -743,33 +744,27 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g, return -1; } - // get sdf + // get sdf plugin int instance = m->geom_plugin[g]; - const int nslot = mjp_pluginCount(); - const int slot = m->plugin[instance]; - const mjpPlugin* sdf = mjp_getPluginAtSlotUnsafe(slot, nslot); - if (!sdf) mjERROR("invalid plugin slot: %d", slot); - if (!(sdf->capabilityflags & mjPLUGIN_SDF)) { - mjERROR("Plugin is not a sign distance field at slot %d", slot); - } + const mjpPlugin* sdf_ptr = instance == -1 ? NULL : mjc_getSDF(m, g); + instance = instance == -1 ? m->geom_dataid[g] : instance; + mjtGeom geomtype = mjGEOM_SDF; + + // construct sdf struct + mjSDF sdf; + sdf.id = &instance; + sdf.type = mjSDFTYPE_SINGLE; + sdf.plugin = &sdf_ptr; + sdf.geomtype = &geomtype; // reset counter - sdf->reset(m, NULL, (void*)(d->plugin_data[instance]), instance); - - // compute transformation - mjtNum sdf_quat[4], sdf_xmat[9], sdf_xpos[9]; - mjtNum negpos[3], negquat[4], xquat[4]; - mjtNum* xpos = d->geom_xpos + 3*g; - mjtNum* pos = m->mesh_pos + 3*m->geom_dataid[g]; - mjtNum* quat = m->mesh_quat + 4*m->geom_dataid[g]; - mju_mat2Quat(xquat, d->geom_xmat + 9*g); - mju_negPose(negpos, negquat, pos, quat); - mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat); - mju_quat2Mat(sdf_xmat, sdf_quat); + if (sdf_ptr) { + sdf_ptr->reset(m, NULL, (void*)(d->plugin_data[instance]), instance); + } // map to local frame mjtNum lpnt[3], lvec[3]; - ray_map(sdf_xpos, sdf_xmat, pnt, vec, lpnt, lvec); + ray_map(d->geom_xpos + 3*g, d->geom_xmat + 9*g, pnt, vec, lpnt, lvec); // unit direction mju_normalize3(lvec); @@ -777,7 +772,7 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g, // ray marching, see e.g. https://en.wikipedia.org/wiki/Ray_marching for (int i=0; i < 40; i++) { mju_addScl3(p, lpnt, lvec, distance_total); - mjtNum distance = sdf->sdf_distance(p, (mjData*)d, instance); + mjtNum distance = mjc_distance(m, d, &sdf, p); distance_total += distance; if (mju_abs(distance) < kMinDist) { return distance_total; @@ -789,7 +784,9 @@ mjtNum ray_sdf(const mjModel* m, const mjData* d, int g, } // reset counter - sdf->reset(m, NULL, (void*)(d->plugin_data[instance]), instance); + if (sdf_ptr) { + sdf_ptr->reset(m, NULL, (void*)(d->plugin_data[instance]), instance); + } return -1; } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index c8a74685..6695f2c0 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -141,6 +141,7 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { processed_ = false; visual_ = true; needoct_ = false; + needreorient_ = true; // reset to default if given if (_def) { @@ -360,6 +361,7 @@ void mjCMesh::LoadSDF() { userface.push_back(index); } + needreorient_ = false; normal_ = std::move(usernormal); face_ = std::move(userface); ProcessVertices(uservert); @@ -1533,22 +1535,26 @@ void mjCMesh::Process() { boxsz_[1] = 0.5 * std::sqrt(6*(eigval[0] + eigval[2] - eigval[1])/volume); boxsz_[2] = 0.5 * std::sqrt(6*(eigval[0] + eigval[1] - eigval[2])/volume); - // make octree in the geom frame - // TODO: make octree in the mesh frame, update engine_collision_sdf + // prevent reorientation if the mesh was autogenerated using marching cubes + if (!needreorient_) { + mjuu_setvec(CoM, 0, 0, 0); + mjuu_setvec(quattmp, 1, 0, 0, 0); + } + + // transform CoM to origin + for (int i=0; i < nvert(); i++) { + vert_[3*i + 0] -= CoM[0]; + vert_[3*i + 1] -= CoM[1]; + vert_[3*i + 2] -= CoM[2]; + } + Rotate(quattmp); + + // make octree in mesh frame if (!needoct_) { octree_.Clear(); } else if (octree_.Nodes().empty()) { - double aamm[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; - for (int i = 0; i < nvert(); i++) { - aamm[0] = std::min(aamm[0], vert_[3*i + 0]); - aamm[3] = std::max(aamm[3], vert_[3*i + 0]); - aamm[1] = std::min(aamm[1], vert_[3*i + 1]); - aamm[4] = std::max(aamm[4], vert_[3*i + 1]); - aamm[2] = std::min(aamm[2], vert_[3*i + 2]); - aamm[5] = std::max(aamm[5], vert_[3*i + 2]); - } octree_.SetFace(vert_, face_); - octree_.CreateOctree(aamm); + octree_.CreateOctree(aamm_); // compute sdf coefficients if (!plugin.active) { @@ -1569,14 +1575,6 @@ void mjCMesh::Process() { } } - // transform CoM to origin - for (int i=0; i < nvert(); i++) { - vert_[3*i + 0] -= CoM[0]; - vert_[3*i + 1] -= CoM[1]; - vert_[3*i + 2] -= CoM[2]; - } - Rotate(quattmp); - // save the pos and quat that was used to transform the mesh mjuu_copyvec(pos_, CoM, 3); mjuu_copyvec(quat_, quattmp, 4); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 98bc5d2d..3060bf6c 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1003,6 +1003,7 @@ class mjCMesh_ : public mjCBase { std::vector spec_facetexcoord_; // used by the compiler + bool needreorient_; // needs reorientation bool needoct_; // needs octree bool visual_; // true: the mesh is only visual std::vector< std::pair > halfedge_; // half-edge data