diff --git a/model/plugin/sdf/cow.xml b/model/plugin/sdf/cow.xml index 0e9528a2..7eb055d4 100644 --- a/model/plugin/sdf/cow.xml +++ b/model/plugin/sdf/cow.xml @@ -8,19 +8,12 @@ - - - - - - - - + @@ -48,15 +41,11 @@ - - - + - - - + diff --git a/plugin/sdf/CMakeLists.txt b/plugin/sdf/CMakeLists.txt index 5a4c66b8..f857bdab 100644 --- a/plugin/sdf/CMakeLists.txt +++ b/plugin/sdf/CMakeLists.txt @@ -28,8 +28,6 @@ set(MUJOCO_SDF_SRCS register.cc nut.cc nut.h - sdflib.cc - sdflib.h torus.cc torus.h ) diff --git a/plugin/sdf/README.md b/plugin/sdf/README.md index b0ebc417..d493a5a6 100644 --- a/plugin/sdf/README.md +++ b/plugin/sdf/README.md @@ -62,16 +62,6 @@ Parameters: - `radius1` [m]: major radius (default `0.35`). - `radius1` [m]: minor radius (default `0.15`). -### SdfLib - -Implemented in [sdflib.cc](sdflib.cc). Example usage in [cow.xml](../../model/plugin/sdf/cow.xml). - -This plugin uses the library [TriangleMeshDistance](https://github.com/InteractiveComputerGraphics/TriangleMeshDistance) -to compute a voxel-based approximation of a user-specified mesh. The mesh can be arbitrary and not necessarily convex. -This offers an alternative to convex-decomposed meshes. The performance is likely to be slower than that of analytic -SDFs, since a cubic approximation has to be evaluated on the convex grid. However, the SDF generation is done -automatically, simplifying the task of creating an SDF, which can be difficult for complex shapes. - ### How to make your own SDF Create your `MySDF.h` and `MySDF.cc` files in the SDF folder, where this README is located. Implement your SDF using the diff --git a/plugin/sdf/register.cc b/plugin/sdf/register.cc index 4b453e18..4e727e38 100644 --- a/plugin/sdf/register.cc +++ b/plugin/sdf/register.cc @@ -17,7 +17,6 @@ #include "gear.h" #include "nut.h" #include "torus.h" -#include "sdflib.h" namespace mujoco::plugin::sdf { @@ -27,7 +26,6 @@ mjPLUGIN_LIB_INIT { Gear::RegisterPlugin(); Nut::RegisterPlugin(); Torus::RegisterPlugin(); - SdfLib::RegisterPlugin(); } } // namespace mujoco::plugin::sdf diff --git a/plugin/sdf/sdflib.cc b/plugin/sdf/sdflib.cc deleted file mode 100644 index 9fae83bd..00000000 --- a/plugin/sdf/sdflib.cc +++ /dev/null @@ -1,279 +0,0 @@ -// Copyright 2022 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include - -#include -#include -#include "sdf.h" -#include "sdflib.h" - -namespace mujoco::plugin::sdf { -namespace { - -mjtNum boxProjection(mjtNum point[3], const mjtNum box[6]) { - mjtNum r[3] = {point[0] - box[0], point[1] - box[1], point[2] - box[2]}; - mjtNum q[3] = {mju_abs(r[0]) - box[3], mju_abs(r[1]) - box[4], - mju_abs(r[2]) - box[5]}; - mjtNum dist_sqr = 0; - mjtNum eps = 1e-6; - - // skip the projection if inside - if (q[0] <= 0 && q[1] <= 0 && q[2] <= 0) { - return mju_max(q[0], mju_max(q[1], q[2])); - } - - // in-place projection inside the box if outside - if ( q[0] >= 0 ) { - dist_sqr += q[0] * q[0]; - point[0] -= r[0] > 0 ? (q[0]+eps) : -(q[0]+eps); - } - if ( q[1] >= 0 ) { - dist_sqr += q[1] * q[1]; - point[1] -= r[1] > 0 ? (q[1]+eps) : -(q[1]+eps); - } - if ( q[2] >= 0 ) { - dist_sqr += q[2] * q[2]; - point[2] -= r[2] > 0 ? (q[2]+eps) : -(q[2]+eps); - } - - return mju_sqrt(dist_sqr); -} - -// find the octree leaf containing the point p, return the index of the leaf and -// populate the weights of the interpolated function (if w is not null) and of -// its gradient (if dw is not null) using the vertices as degrees of freedom for -// trilinear interpolation. -static int findOct(mjtNum w[8], mjtNum dw[8][3], const mjtNum* oct_aabb, - const int* oct_child, const mjtNum p[3]) { - std::vector stack = {0}; - mjtNum eps = 1e-8; - - while (!stack.empty()) { - int node = stack.back(); - stack.pop_back(); - mjtNum vmin[3], vmax[3]; - - if (node == -1) { // SHOULD NOT OCCUR - mju_error("Invalid node number"); - return -1; - } - - for (int j = 0; j < 3; j++) { - vmin[j] = oct_aabb[6*node+j] - oct_aabb[6*node+3+j]; - vmax[j] = oct_aabb[6*node+j] + oct_aabb[6*node+3+j]; - } - - // check if the point is inside the aabb of the octree node - if (p[0] + eps < vmin[0] || p[0] - eps > vmax[0] || - p[1] + eps < vmin[1] || p[1] - eps > vmax[1] || - p[2] + eps < vmin[2] || p[2] - eps > vmax[2]) { - continue; - } - - mjtNum coord[3] = {(p[0] - vmin[0]) / (vmax[0] - vmin[0]), - (p[1] - vmin[1]) / (vmax[1] - vmin[1]), - (p[2] - vmin[2]) / (vmax[2] - vmin[2])}; - - // check if the node is a leaf - if (oct_child[8*node+0] == -1 && oct_child[8*node+1] == -1 && - oct_child[8*node+2] == -1 && oct_child[8*node+3] == -1 && - oct_child[8*node+4] == -1 && oct_child[8*node+5] == -1 && - oct_child[8*node+6] == -1 && oct_child[8*node+7] == -1) { - for (int j = 0; j < 8; j++) { - if (w) { - w[j] = (j & 1 ? coord[0] : 1 - coord[0]) * - (j & 2 ? coord[1] : 1 - coord[1]) * - (j & 4 ? coord[2] : 1 - coord[2]); - } - if (dw) { - dw[j][0] = (j & 1 ? 1 : -1) * - (j & 2 ? coord[1] : 1 - coord[1]) * - (j & 4 ? coord[2] : 1 - coord[2]); - dw[j][1] = (j & 1 ? coord[0] : 1 - coord[0]) * - (j & 2 ? 1 : -1) * - (j & 4 ? coord[2] : 1 - coord[2]); - dw[j][2] = (j & 1 ? coord[0] : 1 - coord[0]) * - (j & 2 ? coord[1] : 1 - coord[1]) * - (j & 4 ? 1 : -1); - } - } - return node; - } - - // compute which of 8 children to visit next - int x = coord[0] < .5 ? 1 : 0; - int y = coord[1] < .5 ? 1 : 0; - int z = coord[2] < .5 ? 1 : 0; - stack.push_back(oct_child[8*node + 4*z + 2*y + x]); - } - - mju_error("Node not found"); // SHOULD NOT OCCUR - return -1; -} - -} // namespace - -// factory function -std::optional SdfLib::Create(const mjModel* m, mjData* d, - int instance) { - int geomid = 0; - for (int i = 0; i < m->ngeom; ++i) { - if (m->geom_plugin[i] == instance) { - geomid = i; - break; - } - } - return SdfLib(m, m->geom_dataid[geomid]); -} - -// plugin constructor -SdfLib::SdfLib(const mjModel* m, int meshid) { - int octadr = m->mesh_octadr[meshid]; - int octnum = m->mesh_octnum[meshid]; - oct_aabb_.assign(m->oct_aabb + 6*octadr, - m->oct_aabb + 6*octadr + 6*octnum); - oct_child_.assign(m->oct_child + 8 * octadr, - m->oct_child + 8 * octadr + 8 * octnum); - sdf_coeff_.assign(8 * octnum, 0); - memcpy(sdf_coeff_.data(), m->oct_coeff + 8*octadr, 8*octnum*sizeof(mjtNum)); - mju_copy(box_, m->oct_aabb + 6*octadr, 6); -} - -// plugin computation -void SdfLib::Compute(const mjModel* m, mjData* d, int instance) { - visualizer_.Next(); -} - -// plugin reset -void SdfLib::Reset() { - visualizer_.Reset(); -} - -// plugin visualization -void SdfLib::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, - mjvScene* scn, int instance) { - visualizer_.Visualize(m, d, opt, scn, instance); -} - -// sdf -mjtNum SdfLib::Distance(const mjtNum p[3]) const { - mjtNum w[8]; - mjtNum sdf = 0; - mjtNum point[3] = {p[0], p[1], p[2]}; - mjtNum boxDist = boxProjection(point, box_); - if (boxDist > 0) { - return boxDist; - } - int node = findOct(w, nullptr, oct_aabb_.data(), oct_child_.data(), point); - for (int i = 0; i < 8; ++i) { - sdf += w[i] * sdf_coeff_[8*node + i]; - } - return sdf; -} - -// gradient of sdf -void SdfLib::Gradient(mjtNum grad[3], const mjtNum point[3]) const { - - mjtNum p[3] = {point[0], point[1], point[2]}; - - // analytic in the interior - if (boxProjection(p, box_) <= 0) { - mjtNum dw[8][3]; - mju_zero3(grad); - int node = findOct(nullptr, dw, oct_aabb_.data(), oct_child_.data(), p); - for (int i = 0; i < 8; ++i) { - grad[0] += dw[i][0] * sdf_coeff_[8*node + i]; - grad[1] += dw[i][1] * sdf_coeff_[8*node + i]; - grad[2] += dw[i][2] * sdf_coeff_[8*node + i]; - } - return; - } - - // finite difference in the exterior - mjtNum eps = 1e-8; - mjtNum dist0 = Distance(point); - mjtNum pointX[3] = {point[0]+eps, point[1], point[2]}; - mjtNum distX = Distance(pointX); - mjtNum pointY[3] = {point[0], point[1]+eps, point[2]}; - mjtNum distY = Distance(pointY); - mjtNum pointZ[3] = {point[0], point[1], point[2]+eps}; - mjtNum distZ = Distance(pointZ); - - grad[0] = (distX - dist0) / eps; - grad[1] = (distY - dist0) / eps; - grad[2] = (distZ - dist0) / eps; -} - -// plugin registration -void SdfLib::RegisterPlugin() { - mjpPlugin plugin; - mjp_defaultPlugin(&plugin); - - plugin.name = "mujoco.sdf.sdflib"; - plugin.capabilityflags |= mjPLUGIN_SDF; - - const char* attributes[] = {"aabb"}; - plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]); - plugin.attributes = attributes; - plugin.nstate = +[](const mjModel* m, int instance) { return 0; }; - - plugin.init = +[](const mjModel* m, mjData* d, int instance) { - auto sdf_or_null = SdfLib::Create(m, d, instance); - if (!sdf_or_null.has_value()) { - return -1; - } - d->plugin_data[instance] = reinterpret_cast( - new SdfLib(std::move(*sdf_or_null))); - return 0; - }; - plugin.destroy = +[](mjData* d, int instance) { - delete reinterpret_cast(d->plugin_data[instance]); - d->plugin_data[instance] = 0; - }; - plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, - int instance) { - auto sdf = reinterpret_cast(plugin_data); - sdf->Reset(); - }; - plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt, - mjvScene* scn, int instance) { - auto* sdf = reinterpret_cast(d->plugin_data[instance]); - sdf->Visualize(m, d, opt, scn, instance); - }; - plugin.compute = - +[](const mjModel* m, mjData* d, int instance, int capability_bit) { - auto* sdf = reinterpret_cast(d->plugin_data[instance]); - sdf->Compute(m, d, instance); - }; - plugin.sdf_distance = - +[](const mjtNum point[3], const mjData* d, int instance) { - auto* sdf = reinterpret_cast(d->plugin_data[instance]); - return sdf->Distance(point); - }; - plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3], - const mjData* d, int instance) { - auto* sdf = reinterpret_cast(d->plugin_data[instance]); - sdf->visualizer_.AddPoint(point); - sdf->Gradient(gradient, point); - }; - - mjp_registerPlugin(&plugin); -} - -} // namespace mujoco::plugin::sdf diff --git a/plugin/sdf/sdflib.h b/plugin/sdf/sdflib.h deleted file mode 100644 index 65def3aa..00000000 --- a/plugin/sdf/sdflib.h +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright 2022 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MUJOCO_PLUGIN_SDF_SDFLIB_H_ -#define MUJOCO_PLUGIN_SDF_SDFLIB_H_ - -#include -#include - -#include -#include -#include -#include -#include "sdf.h" - -namespace mujoco::plugin::sdf { -class SdfLib { - public: - // Creates a new SdfLib instance or returns null on failure. - static std::optional Create(const mjModel* m, mjData* d, - int instance); - SdfLib(SdfLib&&) = default; - ~SdfLib() = default; - - void Reset(); - void Visualize(const mjModel* m, mjData* d, const mjvOption* opt, - mjvScene* scn, int instance); - void Compute(const mjModel* m, mjData* d, int instance); - mjtNum Distance(const mjtNum point[3]) const; - void Gradient(mjtNum grad[3], const mjtNum point[3]) const; - - static void RegisterPlugin(); - - private: - SdfLib(const mjModel* m, int meshid); - SdfVisualizer visualizer_; - std::vector sdf_coeff_; - - mjtNum box_[6]; - std::vector oct_aabb_; - std::vector oct_child_; -}; - -} // namespace mujoco::plugin::sdf - -#endif // MUJOCO_PLUGIN_SDF_SDFLIB_H_ diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index b1942970..a4d62eb7 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -34,6 +34,168 @@ #define MAXMESHPNT 500 +//---------------------------- interpolated sdf ------------------------------------------- + +mjtNum boxProjection(mjtNum point[3], const mjtNum box[6]) { + mjtNum r[3] = {point[0] - box[0], point[1] - box[1], point[2] - box[2]}; + mjtNum q[3] = {mju_abs(r[0]) - box[3], mju_abs(r[1]) - box[4], + mju_abs(r[2]) - box[5]}; + mjtNum dist_sqr = 0; + mjtNum eps = 1e-6; + + // skip the projection if inside + if (q[0] <= 0 && q[1] <= 0 && q[2] <= 0) { + return mju_max(q[0], mju_max(q[1], q[2])); + } + + // in-place projection inside the box if outside + if ( q[0] >= 0 ) { + dist_sqr += q[0] * q[0]; + point[0] -= r[0] > 0 ? (q[0]+eps) : -(q[0]+eps); + } + if ( q[1] >= 0 ) { + dist_sqr += q[1] * q[1]; + point[1] -= r[1] > 0 ? (q[1]+eps) : -(q[1]+eps); + } + if ( q[2] >= 0 ) { + dist_sqr += q[2] * q[2]; + point[2] -= r[2] > 0 ? (q[2]+eps) : -(q[2]+eps); + } + + return mju_sqrt(dist_sqr); +} + +// find the octree leaf containing the point p, return the index of the leaf and +// populate the weights of the interpolated function (if w is not null) and of +// its gradient (if dw is not null) using the vertices as degrees of freedom for +// trilinear interpolation. +static int findOct(mjtNum w[8], mjtNum dw[8][3], const mjtNum* oct_aabb, + const int* oct_child, const mjtNum p[3]) { + int stack = 0; + mjtNum eps = 1e-8; + int niter = 100; + + while (niter-- > 0) { + int node = stack; + mjtNum vmin[3], vmax[3]; + + if (node == -1) { // SHOULD NOT OCCUR + mju_error("Invalid node number"); + return -1; + } + + for (int j = 0; j < 3; j++) { + vmin[j] = oct_aabb[6*node+j] - oct_aabb[6*node+3+j]; + vmax[j] = oct_aabb[6*node+j] + oct_aabb[6*node+3+j]; + } + + // check if the point is inside the aabb of the octree node + if (p[0] + eps < vmin[0] || p[0] - eps > vmax[0] || + p[1] + eps < vmin[1] || p[1] - eps > vmax[1] || + p[2] + eps < vmin[2] || p[2] - eps > vmax[2]) { + continue; + } + + mjtNum coord[3] = {(p[0] - vmin[0]) / (vmax[0] - vmin[0]), + (p[1] - vmin[1]) / (vmax[1] - vmin[1]), + (p[2] - vmin[2]) / (vmax[2] - vmin[2])}; + + // check if the node is a leaf + if (oct_child[8*node+0] == -1 && oct_child[8*node+1] == -1 && + oct_child[8*node+2] == -1 && oct_child[8*node+3] == -1 && + oct_child[8*node+4] == -1 && oct_child[8*node+5] == -1 && + oct_child[8*node+6] == -1 && oct_child[8*node+7] == -1) { + for (int j = 0; j < 8; j++) { + if (w) { + w[j] = (j & 1 ? coord[0] : 1 - coord[0]) * + (j & 2 ? coord[1] : 1 - coord[1]) * + (j & 4 ? coord[2] : 1 - coord[2]); + } + if (dw) { + dw[j][0] = (j & 1 ? 1 : -1) * + (j & 2 ? coord[1] : 1 - coord[1]) * + (j & 4 ? coord[2] : 1 - coord[2]); + dw[j][1] = (j & 1 ? coord[0] : 1 - coord[0]) * + (j & 2 ? 1 : -1) * + (j & 4 ? coord[2] : 1 - coord[2]); + dw[j][2] = (j & 1 ? coord[0] : 1 - coord[0]) * + (j & 2 ? coord[1] : 1 - coord[1]) * + (j & 4 ? 1 : -1); + } + } + return node; + } + + // compute which of 8 children to visit next + int x = coord[0] < .5 ? 1 : 0; + int y = coord[1] < .5 ? 1 : 0; + int z = coord[2] < .5 ? 1 : 0; + stack = oct_child[8 * node + 4*z + 2*y + x]; + } + + mju_error("Node not found"); // SHOULD NOT OCCUR + return -1; +} + +// sdf +mjtNum oct_distance(const mjModel* m, const mjtNum p[3], int meshid) { + int octadr = m->mesh_octadr[meshid]; + int* oct_child = m->oct_child + 8*octadr; + mjtNum* oct_aabb = m->oct_aabb + 6*octadr; + mjtNum* oct_coeff = m->oct_coeff + 8*octadr; + + mjtNum w[8]; + mjtNum sdf = 0; + mjtNum point[3] = {p[0], p[1], p[2]}; + mjtNum boxDist = boxProjection(point, oct_aabb); + if (boxDist > 0) { + return boxDist; + } + int node = findOct(w, NULL, oct_aabb, oct_child, point); + for (int i = 0; i < 8; ++i) { + sdf += w[i] * oct_coeff[8*node + i]; + } + return sdf; +} + +// gradient of sdf +void oct_gradient(const mjModel* m, mjtNum grad[3], const mjtNum point[3], int meshid) { + mju_zero3(grad); + mjtNum p[3] = {point[0], point[1], point[2]}; + + int octadr = m->mesh_octadr[meshid]; + int* oct_child = m->oct_child + 8*octadr; + mjtNum* oct_aabb = m->oct_aabb + 6*octadr; + mjtNum* oct_coeff = m->oct_coeff + 8*octadr; + + // analytic in the interior + if (boxProjection(p, oct_aabb) <= 0) { + mjtNum dw[8][3]; + int node = findOct(NULL, dw, oct_aabb, oct_child, p); + for (int i = 0; i < 8; ++i) { + grad[0] += dw[i][0] * oct_coeff[8*node + i]; + grad[1] += dw[i][1] * oct_coeff[8*node + i]; + grad[2] += dw[i][2] * oct_coeff[8*node + i]; + } + return; + } + + // finite difference in the exterior + mjtNum eps = 1e-8; + mjtNum dist0 = oct_distance(m, point, meshid); + mjtNum pointX[3] = {point[0]+eps, point[1], point[2]}; + mjtNum distX = oct_distance(m, pointX, meshid); + mjtNum pointY[3] = {point[0], point[1]+eps, point[2]}; + mjtNum distY = oct_distance(m, pointY, meshid); + mjtNum pointZ[3] = {point[0], point[1], point[2]+eps}; + mjtNum distZ = oct_distance(m, pointZ, meshid); + + grad[0] = (distX - dist0) / eps; + grad[1] = (distY - dist0) / eps; + grad[2] = (distZ - dist0) / eps; +} + + //---------------------------- primitives sdf --------------------------------------------- static void radialField3d(mjtNum field[3], const mjtNum a[3], const mjtNum x[3], @@ -100,7 +262,11 @@ static mjtNum geomDistance(const mjModel* m, const mjData* d, const mjpPlugin* p b[1] = mju_max(a[1], 0); return mju_min(mju_max(a[0], a[1]), 0) + mju_norm(b, 2); case mjGEOM_SDF: - return p->sdf_distance(x, d, i); + if (p) { + return p->sdf_distance(x, d, i); + } else { + return oct_distance(m, x, i); + } default: mjERROR("sdf collisions not available for geom type %d", type); return 0; @@ -199,7 +365,11 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d, } break; case mjGEOM_SDF: - p->sdf_gradient(gradient, x, d, i); + if (p) { + p->sdf_gradient(gradient, x, d, i); + } else { + oct_gradient(m, gradient, x, i); + } break; default: mjERROR("sdf collisions not available for geom type %d", type); @@ -608,7 +778,8 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g // get sdf plugin int instance = m->geom_plugin[g2]; - const mjpPlugin* sdf_ptr = mjc_getSDF(m, g2); + const mjpPlugin* sdf_ptr = instance == -1 ? NULL : mjc_getSDF(m, g2); + instance = instance == -1 ? m->geom_dataid[g2] : instance; mjtGeom geomtype = mjGEOM_SDF; // copy into data @@ -750,22 +921,26 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m mjtGeom geomtypes[2] = {m->geom_type[g2], m->geom_type[g1]}; instance[0] = m->geom_plugin[g2]; - sdf_ptr[0] = mjc_getSDF(m, g2); + sdf_ptr[0] = instance[0] == -1 ? NULL : mjc_getSDF(m, g2); // get sdf plugins if (m->geom_type[g1] == mjGEOM_SDF) { instance[1] = m->geom_plugin[g1]; - sdf_ptr[1] = mjc_getSDF(m, g1); + sdf_ptr[1] = instance[1] == -1 ? NULL : mjc_getSDF(m, g1); } else { instance[1] = g1; sdf_ptr[1] = NULL; } // reset visualization count - sdf_ptr[0]->reset(m, NULL, (void*)(d->plugin_data[instance[0]]), instance[0]); + if (sdf_ptr[0]) { + sdf_ptr[0]->reset(m, NULL, (void*)(d->plugin_data[instance[0]]), instance[0]); + } // copy into sdf mjSDF sdf; + instance[0] = instance[0] == -1 ? m->geom_dataid[g2] : instance[0]; + instance[1] = instance[1] == -1 ? m->geom_dataid[g1] : instance[1]; sdf.id = instance; sdf.relpos = offset21; sdf.relmat = rotation21; @@ -794,7 +969,9 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m i++; // start counters - sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF); + if (sdf_ptr[0]) { + sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF); + } // gradient descent - we use a special function of the two SDF as objective sdf.type = mjSDFTYPE_COLLISION; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index facb78d2..c8a74685 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -1551,8 +1551,7 @@ void mjCMesh::Process() { octree_.CreateOctree(aamm); // compute sdf coefficients - // TODO: only check !plugin.active once sdflib is removed - if (plugin.active && *plugin.name == "sdf") { + if (!plugin.active) { tmd::TriangleMeshDistance sdf(vert_.data(), nvert(), face_.data(), nface()); // TODO: do not evaluate the SDF multiple times at the same vertex diff --git a/test/engine/engine_plugin_test.cc b/test/engine/engine_plugin_test.cc index 5463f1e2..725d0c6f 100644 --- a/test/engine/engine_plugin_test.cc +++ b/test/engine/engine_plugin_test.cc @@ -37,7 +37,7 @@ using ::testing::DoubleNear; using ::testing::HasSubstr; using ::testing::NotNull; -constexpr int kNumTruePlugins = 10; +constexpr int kNumTruePlugins = 9; constexpr int kNumFakePlugins = 30; constexpr int kNumTestPlugins = 4;