1313// limitations under the License.
1414
1515#include < cstdint>
16+ #include < cstring>
1617#include < optional>
1718#include < utility>
1819#include < vector>
1920
20- #include < SdfLib/utils/Mesh.h>
21- #include < SdfLib/OctreeSdf.h>
21+ #include < TriangleMeshDistance/include/tmd/TriangleMeshDistance.h>
2222#include < mujoco/mjplugin.h>
2323#include < mujoco/mujoco.h>
2424#include " sdf.h"
2727namespace mujoco ::plugin::sdf {
2828namespace {
2929
30- inline unsigned int * MakeNonConstUnsigned (const int * ptr) {
31- return reinterpret_cast <unsigned int *>(const_cast <int *>(ptr));
32- }
33-
34- mjtNum boxProjection (glm::vec3& point, const sdflib::BoundingBox& box) {
35- glm::vec3 r = point - box.getCenter ();
36- glm::vec3 q = glm::abs (r) - 0 .5f * box.getSize ();
30+ mjtNum boxProjection (mjtNum point[3 ], const mjtNum box[6 ]) {
31+ mjtNum r[3 ] = {point[0 ] - box[0 ], point[1 ] - box[1 ], point[2 ] - box[2 ]};
32+ mjtNum q[3 ] = {mju_abs (r[0 ]) - box[3 ], mju_abs (r[1 ]) - box[4 ],
33+ mju_abs (r[2 ]) - box[5 ]};
3734 mjtNum dist_sqr = 0 ;
3835 mjtNum eps = 1e-6 ;
3936
4037 // skip the projection if inside
41- if (q. x <= 0 && q. y <= 0 && q. z <= 0 ) {
42- return glm::max (q. x , glm::max (q. y , q. z ));
38+ if (q[ 0 ] <= 0 && q[ 1 ] <= 0 && q[ 2 ] <= 0 ) {
39+ return mju_max (q[ 0 ], mju_max (q[ 1 ] , q[ 2 ] ));
4340 }
4441
4542 // in-place projection inside the box if outside
46- if ( q. x >= 0 ) {
47- dist_sqr += q. x * q. x ;
48- point. x -= r. x > 0 ? (q. x +eps) : -(q. x +eps);
43+ if ( q[ 0 ] >= 0 ) {
44+ dist_sqr += q[ 0 ] * q[ 0 ] ;
45+ point[ 0 ] -= r[ 0 ] > 0 ? (q[ 0 ] +eps) : -(q[ 0 ] +eps);
4946 }
50- if ( q. y >= 0 ) {
51- dist_sqr += q. y * q. y ;
52- point. y -= r. y > 0 ? (q. y +eps) : -(q. y +eps);
47+ if ( q[ 1 ] >= 0 ) {
48+ dist_sqr += q[ 1 ] * q[ 1 ] ;
49+ point[ 1 ] -= r[ 1 ] > 0 ? (q[ 1 ] +eps) : -(q[ 1 ] +eps);
5350 }
54- if ( q. z >= 0 ) {
55- dist_sqr += q. z * q. z ;
56- point. z -= r. z > 0 ? (q. z +eps) : -(q. z +eps);
51+ if ( q[ 2 ] >= 0 ) {
52+ dist_sqr += q[ 2 ] * q[ 2 ] ;
53+ point[ 2 ] -= r[ 2 ] > 0 ? (q[ 2 ] +eps) : -(q[ 2 ] +eps);
5754 }
5855
5956 return mju_sqrt (dist_sqr);
6057}
6158
59+ // find the octree leaf containing the point p, return the index of the leaf and
60+ // populate the weights of the interpolated function (if w is not null) and of
61+ // its gradient (if dw is not null) using the vertices as degrees of freedom for
62+ // trilinear interpolation.
63+ static int findOct (mjtNum w[8 ], mjtNum dw[8 ][3 ], const mjtNum* oct_aabb,
64+ const int * oct_child, const mjtNum p[3 ]) {
65+ std::vector<int > stack = {0 };
66+ mjtNum eps = 1e-8 ;
67+
68+ while (!stack.empty ()) {
69+ int node = stack.back ();
70+ stack.pop_back ();
71+ mjtNum vmin[3 ], vmax[3 ];
72+
73+ if (node == -1 ) { // SHOULD NOT OCCUR
74+ mju_error (" Invalid node number"
75+ return -1 ;
76+ }
77+
78+ for (int j = 0 ; j < 3 ; j++) {
79+ vmin[j] = oct_aabb[6 *node+j] - oct_aabb[6 *node+3 +j];
80+ vmax[j] = oct_aabb[6 *node+j] + oct_aabb[6 *node+3 +j];
81+ }
82+
83+ // check if the point is inside the aabb of the octree node
84+ if (p[0 ] + eps < vmin[0 ] || p[0 ] - eps > vmax[0 ] ||
85+ p[1 ] + eps < vmin[1 ] || p[1 ] - eps > vmax[1 ] ||
86+ p[2 ] + eps < vmin[2 ] || p[2 ] - eps > vmax[2 ]) {
87+ continue ;
88+ }
89+
90+ mjtNum coord[3 ] = {(p[0 ] - vmin[0 ]) / (vmax[0 ] - vmin[0 ]),
91+ (p[1 ] - vmin[1 ]) / (vmax[1 ] - vmin[1 ]),
92+ (p[2 ] - vmin[2 ]) / (vmax[2 ] - vmin[2 ])};
93+
94+ // check if the node is a leaf
95+ if (oct_child[8 *node+0 ] == -1 && oct_child[8 *node+1 ] == -1 &&
96+ oct_child[8 *node+2 ] == -1 && oct_child[8 *node+3 ] == -1 &&
97+ oct_child[8 *node+4 ] == -1 && oct_child[8 *node+5 ] == -1 &&
98+ oct_child[8 *node+6 ] == -1 && oct_child[8 *node+7 ] == -1 ) {
99+ for (int j = 0 ; j < 8 ; j++) {
100+ if (w) {
101+ w[j] = (j & 1 ? coord[0 ] : 1 - coord[0 ]) *
102+ (j & 2 ? coord[1 ] : 1 - coord[1 ]) *
103+ (j & 4 ? coord[2 ] : 1 - coord[2 ]);
104+ }
105+ if (dw) {
106+ dw[j][0 ] = (j & 1 ? 1 : -1 ) *
107+ (j & 2 ? coord[1 ] : 1 - coord[1 ]) *
108+ (j & 4 ? coord[2 ] : 1 - coord[2 ]);
109+ dw[j][1 ] = (j & 1 ? coord[0 ] : 1 - coord[0 ]) *
110+ (j & 2 ? 1 : -1 ) *
111+ (j & 4 ? coord[2 ] : 1 - coord[2 ]);
112+ dw[j][2 ] = (j & 1 ? coord[0 ] : 1 - coord[0 ]) *
113+ (j & 2 ? coord[1 ] : 1 - coord[1 ]) *
114+ (j & 4 ? 1 : -1 );
115+ }
116+ }
117+ return node;
118+ }
119+
120+ // compute which of 8 children to visit next
121+ int x = coord[0 ] < .5 ? 1 : 0 ;
122+ int y = coord[1 ] < .5 ? 1 : 0 ;
123+ int z = coord[2 ] < .5 ? 1 : 0 ;
124+ stack.push_back (oct_child[8 *node + 4 *z + 2 *y + x]);
125+ }
126+
127+ mju_error (" Node not found" // SHOULD NOT OCCUR
128+ return -1 ;
129+ }
130+
62131} // namespace
63132
64133// factory function
@@ -76,30 +145,40 @@ std::optional<SdfLib> SdfLib::Create(const mjModel* m, mjData* d,
76145 int nface = m->mesh_facenum [meshid];
77146 int * indices = m->mesh_face + 3 *m->mesh_faceadr [meshid];
78147 float * verts = m->mesh_vert + 3 *m->mesh_vertadr [meshid];
79- std::vector<glm::vec3 > vertices (nvert);
148+ std::vector<double > vertices (3 * nvert);
80149 for (int i = 0 ; i < nvert; i++) {
81150 mjtNum vert[3 ] = {verts[3 *i+0 ], verts[3 *i+1 ], verts[3 *i+2 ]};
82151 mju_rotVecQuat (vert, vert, m->mesh_quat + 4 *meshid);
83152 mju_addTo3 (vert, m->mesh_pos + 3 *meshid);
84- vertices[i]. x = vert[0 ];
85- vertices[i]. y = vert[1 ];
86- vertices[i]. z = vert[2 ];
153+ vertices[3 *i+ 0 ] = vert[0 ];
154+ vertices[3 *i+ 1 ] = vert[1 ];
155+ vertices[3 *i+ 2 ] = vert[2 ];
87156 }
88- sdflib::Mesh mesh (vertices.data (), nvert,
89- MakeNonConstUnsigned (indices), 3 *nface);
90- mesh.computeBoundingBox ();
91- return SdfLib (std::move (mesh));
157+ tmd::TriangleMeshDistance mesh (vertices.data (), nvert, indices, nface);
158+ return SdfLib (mesh, m, meshid);
92159}
93160
94161// plugin constructor
95- SdfLib::SdfLib (sdflib::Mesh&& mesh) {
96- sdflib::BoundingBox box = mesh.getBoundingBox ();
97- const glm::vec3 modelBBsize = box.getSize ();
98- box.addMargin (
99- 0 .1f * glm::max (glm::max (modelBBsize.x , modelBBsize.y ), modelBBsize.z ));
100- sdf_func_ =
101- sdflib::OctreeSdf (mesh, box, 8 , 3 , 1e-3 ,
102- sdflib::OctreeSdf::InitAlgorithm::CONTINUITY, 1 );
162+ SdfLib::SdfLib (const tmd::TriangleMeshDistance& sdf, const mjModel* m,
163+ int meshid) {
164+ // TODO: do not evaluate the SDF multiple times at the same vertex
165+ // TODO: the value at hanging vertices should be computed from the parent
166+ int octadr = m->mesh_octadr [meshid];
167+ int octnum = m->mesh_octnum [meshid];
168+ oct_aabb_.assign (m->oct_aabb + 6 *octadr,
169+ m->oct_aabb + 6 *octadr + 6 *octnum);
170+ oct_child_.assign (m->oct_child + 8 * octadr,
171+ m->oct_child + 8 * octadr + 8 * octnum);
172+ for (int i = 0 ; i < octnum; ++i) {
173+ for (int j = 0 ; j < 8 ; j++) {
174+ mjtNum v[3 ];
175+ v[0 ] = oct_aabb_[6 *i+0 ] + (j&1 ? 1 : -1 ) * oct_aabb_[6 *i+3 ];
176+ v[1 ] = oct_aabb_[6 *i+1 ] + (j&2 ? 1 : -1 ) * oct_aabb_[6 *i+4 ];
177+ v[2 ] = oct_aabb_[6 *i+2 ] + (j&4 ? 1 : -1 ) * oct_aabb_[6 *i+5 ];
178+ sdf_coeff_.push_back (sdf.signed_distance (v).distance );
179+ }
180+ }
181+ mju_copy (box_, m->oct_aabb + 6 *octadr, 6 );
103182}
104183
105184// plugin computation
@@ -120,22 +199,35 @@ void SdfLib::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
120199
121200// sdf
122201mjtNum SdfLib::Distance (const mjtNum p[3 ]) const {
123- glm::vec3 point (p[0 ], p[1 ], p[2 ]);
124- mjtNum boxDist = boxProjection (point, sdf_func_.getGridBoundingBox ());
125- return sdf_func_.getDistance (point) + (boxDist <= 0 ? 0 : boxDist);
202+ mjtNum w[8 ];
203+ mjtNum sdf = 0 ;
204+ mjtNum point[3 ] = {p[0 ], p[1 ], p[2 ]};
205+ mjtNum boxDist = boxProjection (point, box_);
206+ if (boxDist > 0 ) {
207+ return boxDist;
208+ }
209+ int node = findOct (w, nullptr , oct_aabb_.data (), oct_child_.data (), point);
210+ for (int i = 0 ; i < 8 ; ++i) {
211+ sdf += w[i] * sdf_coeff_[8 *node + i];
212+ }
213+ return sdf;
126214}
127215
128216// gradient of sdf
129217void SdfLib::Gradient (mjtNum grad[3 ], const mjtNum point[3 ]) const {
130- glm::vec3 gradient;
131- glm::vec3 p ( point[0 ], point[1 ], point[2 ]) ;
218+
219+ mjtNum p[ 3 ] = { point[0 ], point[1 ], point[2 ]} ;
132220
133221 // analytic in the interior
134- if (boxProjection (p, sdf_func_.getGridBoundingBox ()) <= 0 ) {
135- sdf_func_.getDistance (p, gradient);
136- grad[0 ] = gradient[0 ];
137- grad[1 ] = gradient[1 ];
138- grad[2 ] = gradient[2 ];
222+ if (boxProjection (p, box_) <= 0 ) {
223+ mjtNum dw[8 ][3 ];
224+ mju_zero3 (grad);
225+ int node = findOct (nullptr , dw, oct_aabb_.data (), oct_child_.data (), p);
226+ for (int i = 0 ; i < 8 ; ++i) {
227+ grad[0 ] += dw[i][0 ] * sdf_coeff_[8 *node + i];
228+ grad[1 ] += dw[i][1 ] * sdf_coeff_[8 *node + i];
229+ grad[2 ] += dw[i][2 ] * sdf_coeff_[8 *node + i];
230+ }
139231 return ;
140232 }
141233
0 commit comments