From fb259a5edd6ab31b0cde3c8d295b13707f07f413 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 2 Jul 2026 00:02:24 -0700 Subject: [PATCH] Implement RenderableManager. Similar to LightManager, the RenderableManager manages renderables based on an mjModel and its mjData. PiperOrigin-RevId: 941510745 Change-Id: I97b44cc857f136aa7c90ceb85a42e299fc759b73 --- src/engine/engine_sleep.h | 2 +- src/engine/engine_vis_visualize.h | 4 +- src/render/filament/CMakeLists.txt | 4 + src/render/filament/support/mesh_util.cc | 589 ++++++++++ src/render/filament/support/mesh_util.h | 45 + .../filament/support/renderable_manager.cc | 1009 +++++++++++++++++ .../filament/support/renderable_manager.h | 100 ++ 7 files changed, 1750 insertions(+), 3 deletions(-) create mode 100644 src/render/filament/support/mesh_util.cc create mode 100644 src/render/filament/support/mesh_util.h create mode 100644 src/render/filament/support/renderable_manager.cc create mode 100644 src/render/filament/support/renderable_manager.h diff --git a/src/engine/engine_sleep.h b/src/engine/engine_sleep.h index b3d6dbf7..45e1febe 100644 --- a/src/engine/engine_sleep.h +++ b/src/engine/engine_sleep.h @@ -31,7 +31,7 @@ void mj_updateSleepInit(const mjModel* m, mjData* d, int flg_staticawake); MJAPI void mj_updateSleep(const mjModel* m, mjData* d); // return the first tree in the sleep cycle that starts at i, -1 if error -int mj_sleepCycle(const int* tree_asleep, int ntree, int i); +MJAPI int mj_sleepCycle(const int* tree_asleep, int ntree, int i); // wake tree i and its related island cycle, return number of woke trees MJAPI int mj_wakeIsland(int* tree_asleep, int ntree, int i, int wakeval, diff --git a/src/engine/engine_vis_visualize.h b/src/engine/engine_vis_visualize.h index f70299d7..f5bd51ea 100644 --- a/src/engine/engine_vis_visualize.h +++ b/src/engine/engine_vis_visualize.h @@ -69,8 +69,8 @@ MJAPI void mjv_cameraFrustum(float zver[2], float zhor[2], float zclip[2], cons MJAPI int mjv_isCatenary(const mjModel* m, const mjData* d, int i, mjtNum* length); // computes points along a catenary curve -int mjv_catenary(const mjtNum x0[3], const mjtNum x1[3], const mjtNum gravity[3], mjtNum length, - mjtNum* catenary, int ncatenary); +MJAPI int mjv_catenary(const mjtNum x0[3], const mjtNum x1[3], const mjtNum gravity[3], + mjtNum length, mjtNum* catenary, int ncatenary); // convert HSV to RGB MJAPI void hsv2rgb(float *RGB, float H, float S, float V); diff --git a/src/render/filament/CMakeLists.txt b/src/render/filament/CMakeLists.txt index f0e565f2..edc2937d 100644 --- a/src/render/filament/CMakeLists.txt +++ b/src/render/filament/CMakeLists.txt @@ -57,8 +57,12 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} support/filament_util.cc support/light_manager.h support/light_manager.cc + support/mesh_util.h + support/mesh_util.cc support/model_objects.h support/model_objects.cc + support/renderable_manager.h + support/renderable_manager.cc ) target_include_directories(${MUJOCO_FILAMENT_TARGET_NAME} diff --git a/src/render/filament/support/mesh_util.cc b/src/render/filament/support/mesh_util.cc new file mode 100644 index 00000000..d72ac09a --- /dev/null +++ b/src/render/filament/support/mesh_util.cc @@ -0,0 +1,589 @@ +// Copyright 2026 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 "render/filament/support/mesh_util.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include "engine/engine_vis_visualize.h" +#include "render/filament/mjrfilament_cpp.h" +#include "render/filament/support/filament_util.h" + +namespace mujoco { + +using filament::math::float2; +using filament::math::float3; +using filament::math::float4; + +// Vertex types that can be used to fill in mesh data using generic functions. +namespace { + +struct VertexNoUv { + float3 position; + float4 orientation; + static constexpr bool kHasUv = false; +}; + +struct VertexWithUv { + float3 position; + float4 orientation; + float2 uv; + static constexpr bool kHasUv = true; +}; + +} // namespace + + +static void AccumulateNormal(float3* normals, const mjtNum* src_positions, + const int* indices, int i0, int i1, int i2) { + const int idx0 = indices[i0]; + const int idx1 = indices[i1]; + const int idx2 = indices[i2]; + + const float3 v0 = ReadFloat3(src_positions, idx0); + const float3 v1 = ReadFloat3(src_positions, idx1); + const float3 v2 = ReadFloat3(src_positions, idx2); + const float3 normal = cross(v1 - v0, v2 - v0); + + normals[idx0] += normal; + normals[idx1] += normal; + normals[idx2] += normal; +} + +template +static void AddFlatFace(T* vertices, const mjtNum* src_positions, float radius, + const int* indices, int i0, int i1, int i2) { + const float3 v0 = ReadFloat3(src_positions, indices[i0]); + const float3 v1 = ReadFloat3(src_positions, indices[i1]); + const float3 v2 = ReadFloat3(src_positions, indices[i2]); + + const float3 normal = normalize(cross(v1 - v0, v2 - v0)); + vertices[0].position = v0 + (radius * normal); + vertices[1].position = v1 + (radius * normal); + vertices[2].position = v2 + (radius * normal); + + const float4 orientation = CalculateOrientation(normal); + vertices[0].orientation = orientation; + vertices[1].orientation = orientation; + vertices[2].orientation = orientation; +} + +template +static void AddSmoothFace(T* vertices, const mjtNum* src_positions, + const float3* src_normals, float radius, + const int* indices, int i0, int i1, int i2) { + const int idx0 = indices[i0]; + const int idx1 = indices[i1]; + const int idx2 = indices[i2]; + + const float3 v0 = ReadFloat3(src_positions, idx0); + const float3 v1 = ReadFloat3(src_positions, idx1); + const float3 v2 = ReadFloat3(src_positions, idx2); + + vertices[0].position = v0 + (radius * src_normals[idx0]); + vertices[1].position = v1 + (radius * src_normals[idx1]); + vertices[2].position = v2 + (radius * src_normals[idx2]); + + const float sign = radius > 0 ? 1.f : -1.f; + vertices[0].orientation = CalculateOrientation(sign * src_normals[idx0]); + vertices[1].orientation = CalculateOrientation(sign * src_normals[idx1]); + vertices[2].orientation = CalculateOrientation(sign * src_normals[idx2]); +} + +// Assumes T::orientation is already set to the "smoothed" normal. +template +static void AddSideFace(T* vertices, const mjtNum* src_positions, + const float3* src_normals, float radius, + const int* indices, int i0, int i1) { + const int idx0 = indices[i0]; + const int idx1 = indices[i1]; + + const float3 v0 = ReadFloat3(src_positions, idx0); + const float3 v1 = ReadFloat3(src_positions, idx1); + const float3 v01 = v1 - v0; + float3 normal = normalize(cross(v01, src_normals[idx1])); + if (radius < 0) { + normal = -normal; + } + + vertices[0].position = v0 + radius * src_normals[idx0]; + vertices[1].position = v1 - radius * src_normals[idx1]; + vertices[2].position = v1 + radius * src_normals[idx1]; + + vertices[0].orientation = CalculateOrientation(normal); + vertices[1].orientation = vertices[0].orientation; + vertices[2].orientation = vertices[0].orientation; +} + +template +static void AddFaceUvs(T* vertices, const float* src_uvs, const int* indices, + int t0, int t1, int t2) { + if constexpr (T::kHasUv) { + if (src_uvs) { + vertices[0].uv = ReadFloat2(src_uvs, indices[t0]); + vertices[1].uv = ReadFloat2(src_uvs, indices[t1]); + vertices[2].uv = ReadFloat2(src_uvs, indices[t2]); + } + } +} + +template +static void UpdateBounds(float3* min_pt, float3* max_pt, const T* vertices) { + *min_pt = min(*min_pt, vertices[0].position); + *max_pt = max(*max_pt, vertices[0].position); + *min_pt = min(*min_pt, vertices[1].position); + *max_pt = max(*max_pt, vertices[1].position); + *min_pt = min(*min_pt, vertices[2].position); + *max_pt = max(*max_pt, vertices[2].position); +} + +static int CalculateVertexCount(const mjModel* model, int flex_id, + int flex_layer, bool smooth_skinning) { + const int dim = model->flex_dim[flex_id]; + + int num_faces = 0; + if (dim == 1) { + // 1d flexes have no faces. + } else if (smooth_skinning) { + if (dim == 2) { + num_faces += (2 * model->flex_elemnum[flex_id]); + num_faces += (2 * model->flex_shellnum[flex_id]); + } else { + num_faces += model->flex_shellnum[flex_id]; + } + } else { + if (dim == 2) { + num_faces += (2 * model->flex_elemnum[flex_id]); + } else { + for (int e = 0; e < model->flex_elemnum[flex_id]; e++) { + if (model->flex_elemlayer[model->flex_elemadr[flex_id] + e] == + flex_layer) { + num_faces += 4; + } + } + } + } + return num_faces * 3; +} + +template +static void FillFlexVertices(T* vertices, const mjModel* model, + const mjData* data, int flex_id, float3* min_pt, + float3* max_pt, int flex_layer, + bool smooth_skinning) { + *min_pt = float3(FLT_MAX); + *max_pt = float3(FLT_MIN); + + const int num_vertices = + CalculateVertexCount(model, flex_id, flex_layer, smooth_skinning); + const int dim = model->flex_dim[flex_id]; + const float radius = (float)model->flex_radius[flex_id]; + const bool flat_shading = (bool)model->flex_flatskin[flex_id]; + + const mjtNum* src_positions = + data->flexvert_xpos + 3 * model->flex_vertadr[flex_id]; + + const float* src_uvs = nullptr; + if (model->flex_texcoordadr[flex_id] >= 0) { + src_uvs = model->flex_texcoord + 2 * model->flex_texcoordadr[flex_id]; + } + + const int* edata = model->flex_elem + model->flex_elemdataadr[flex_id]; + const int* sdata = model->flex_shell + model->flex_shelldataadr[flex_id]; + const int* tdata = + model->flex_elemtexcoord + model->flex_elemdataadr[flex_id]; + + if (dim == 1) { + // 1D - don't render? + return; + } + + else if (smooth_skinning) { + // Accumulate normals in the `orientation` field. + std::vector normals(num_vertices, float3(0, 0, 0)); + if (dim == 2) { + for (int e = 0; e < model->flex_elemnum[flex_id]; ++e) { + const int* indices = edata + e * (dim + 1); + AccumulateNormal(normals.data(), src_positions, indices, 0, 1, 2); + } + } else { + for (int s = 0; s < model->flex_shellnum[flex_id]; ++s) { + const int* indices = sdata + s * dim; + AccumulateNormal(normals.data(), src_positions, indices, 0, 1, 2); + } + } + + // Normalize the accumulated normals. + for (float3& n : normals) { + n = normalize(n); + } + + if (dim == 2) { + for (int e = 0; e < model->flex_elemnum[flex_id]; ++e) { + const int* indices = edata + (e * (dim + 1)); + const int* tex_indices = tdata + (e * (dim + 1)); + + if (flat_shading) { + AddFlatFace(vertices, src_positions, radius, indices, 0, 1, 2); + } else { + AddSmoothFace(vertices, src_positions, normals.data(), radius, + indices, 0, 1, 2); + } + AddFaceUvs(vertices, src_uvs, tex_indices, 0, 1, 2); + UpdateBounds(min_pt, max_pt, vertices); + vertices += 3; + + if (flat_shading) { + AddFlatFace(vertices, src_positions, -radius, indices, 0, 2, 1); + } else { + AddSmoothFace(vertices, src_positions, normals.data(), -radius, + indices, 0, 2, 1); + } + AddFaceUvs(vertices, src_uvs, tex_indices, 0, 2, 1); + UpdateBounds(min_pt, max_pt, vertices); + vertices += 3; + } + for (int s = 0; s < model->flex_shellnum[flex_id]; ++s) { + const int* indices = sdata + (s * dim); + + AddSideFace(vertices, src_positions, normals.data(), radius, indices, 0, + 1); + AddFaceUvs(vertices, src_uvs, indices, 0, 1, 1); + UpdateBounds(min_pt, max_pt, vertices); + vertices += 3; + + AddSideFace(vertices, src_positions, normals.data(), -radius, indices, + 1, 0); + AddFaceUvs(vertices, src_uvs, indices, 1, 0, 0); + UpdateBounds(min_pt, max_pt, vertices); + vertices += 3; + } + } else { + for (int s = 0; s < model->flex_shellnum[flex_id]; ++s) { + const int* indices = sdata + s * dim; + if (flat_shading) { + AddFlatFace(vertices, src_positions, radius, indices, 0, 1, 2); + } else { + AddSmoothFace(vertices, src_positions, normals.data(), radius, + indices, 0, 1, 2); + } + AddFaceUvs(vertices, src_uvs, indices, 0, 1, 2); + UpdateBounds(min_pt, max_pt, vertices); + vertices += 3; + } + } + } + + // 2D or 3D face: faces from elements, flat normals, texture + else { + for (int e = 0; e < model->flex_elemnum[flex_id]; e++) { + // in 3D, show only elements in selected layer + if (dim == 2 || model->flex_elemlayer[model->flex_elemadr[flex_id] + e] == + flex_layer) { + const int* edata2 = edata + e * (dim + 1); + const int* tdata2 = tdata + e * (dim + 1); + + if (dim == 2) { + AddFlatFace(vertices, src_positions, radius, edata2, 0, 1, 2); + AddFaceUvs(vertices, src_uvs, tdata2, 0, 1, 2); + UpdateBounds(min_pt, max_pt, vertices); + vertices += 3; + + AddFlatFace(vertices, src_positions, radius, edata2, 0, 2, 1); + AddFaceUvs(vertices, src_uvs, tdata2, 0, 2, 1); + UpdateBounds(min_pt, max_pt, vertices); + vertices += 3; + } else { + AddFlatFace(vertices, src_positions, radius, edata2, 0, 1, 2); + AddFaceUvs(vertices, src_uvs, tdata2, 0, 1, 2); + UpdateBounds(min_pt, max_pt, vertices); + vertices += 3; + + AddFlatFace(vertices, src_positions, radius, edata2, 0, 2, 3); + AddFaceUvs(vertices, src_uvs, tdata2, 0, 2, 3); + UpdateBounds(min_pt, max_pt, vertices); + vertices += 3; + + AddFlatFace(vertices, src_positions, radius, edata2, 0, 3, 1); + AddFaceUvs(vertices, src_uvs, tdata2, 0, 3, 1); + UpdateBounds(min_pt, max_pt, vertices); + vertices += 3; + + AddFlatFace(vertices, src_positions, radius, edata2, 1, 3, 2); + AddFaceUvs(vertices, src_uvs, tdata2, 1, 3, 2); + UpdateBounds(min_pt, max_pt, vertices); + vertices += 3; + } + } + } + } +} + +template +static void FillSkinVertices(T* vertices, const mjModel* model, + const mjData* data, int skin_id, float3* min_pt, + float3* max_pt) { + const int vertadr = model->skin_vertadr[skin_id]; + const int faceadr = model->skin_faceadr[skin_id]; + const int facenum = model->skin_facenum[skin_id]; + const int boneadr = model->skin_boneadr[skin_id]; + const int bonenum = model->skin_bonenum[skin_id]; + + // Accumulate positions from all bones. + for (int bone_idx = boneadr; bone_idx < boneadr + bonenum; ++bone_idx) { + mjtNum bind_pos[3] = {(mjtNum)model->skin_bonebindpos[3 * bone_idx + 0], + (mjtNum)model->skin_bonebindpos[3 * bone_idx + 1], + (mjtNum)model->skin_bonebindpos[3 * bone_idx + 2]}; + mjtNum bind_quat[4] = {(mjtNum)model->skin_bonebindquat[4 * bone_idx + 0], + (mjtNum)model->skin_bonebindquat[4 * bone_idx + 1], + (mjtNum)model->skin_bonebindquat[4 * bone_idx + 2], + (mjtNum)model->skin_bonebindquat[4 * bone_idx + 3]}; + + const int body_id = model->skin_bonebodyid[bone_idx]; + const mjtNum* body_quat = data->xquat + 4 * body_id; + const mjtNum* body_pos = data->xpos + 3 * body_id; + + // Apply the bone's current pose to the bind pose. + mjtNum neg_bind_quat[4]; + mju_negQuat(neg_bind_quat, bind_quat); + + mjtNum quat[4]; + mju_mulQuat(quat, body_quat, neg_bind_quat); + + mjtNum rotate[9]; + mju_quat2Mat(rotate, quat); + + mjtNum translate[3]; + mju_mulMatVec3(translate, rotate, bind_pos); + mju_sub3(translate, body_pos, translate); + + // Apply the bone's position to all vertices "connected" to the bone by + // the weighting of the bone to the vertex. + const int bonevertadr = model->skin_bonevertadr[bone_idx]; + const int bonevertnum = model->skin_bonevertnum[bone_idx]; + for (int i = bonevertadr; i < bonevertadr + bonevertnum; ++i) { + const int vertex_id = model->skin_bonevertid[i]; + const mjtNum base_pos[3] = { + (mjtNum)model->skin_vert[3 * (vertadr + vertex_id) + 0], + (mjtNum)model->skin_vert[3 * (vertadr + vertex_id) + 1], + (mjtNum)model->skin_vert[3 * (vertadr + vertex_id) + 2], + }; + mjtNum unweighted_pos[3]; + mju_mulMatVec3(unweighted_pos, rotate, base_pos); + mju_addTo3(unweighted_pos, translate); + + const float weight = model->skin_bonevertweight[i]; + vertices[vertex_id].position.x += weight * (float)unweighted_pos[0]; + vertices[vertex_id].position.y += weight * (float)unweighted_pos[1]; + vertices[vertex_id].position.z += weight * (float)unweighted_pos[2]; + } + } + + // Compute normals for each face. For now, we'll store the normals in the + // xyz components of the orientation field. + for (int i = faceadr; i < faceadr + facenum; ++i) { + const int i0 = model->skin_face[(3 * i) + 0]; + const int i1 = model->skin_face[(3 * i) + 1]; + const int i2 = model->skin_face[(3 * i) + 2]; + T& v0 = vertices[i0]; + T& v1 = vertices[i1]; + T& v2 = vertices[i2]; + + const float3 vec01 = v1.position - v0.position; + const float3 vec02 = v2.position - v0.position; + const float4 normal = float4(cross(vec01, vec02), 0); + + v0.orientation += normal; + v1.orientation += normal; + v2.orientation += normal; + } + + const float* uvs = nullptr; + if (model->skin_texcoordadr[skin_id] >= 0) { + const int uaddr = model->skin_texcoordadr[skin_id]; + uvs = model->skin_texcoord + (2 * uaddr); + } + + // Perform final adjustments/corrections on all the vertices. + const float inflate = model->skin_inflate[skin_id]; + const size_t num_vertices = model->skin_vertnum[skin_id]; + for (int i = 0; i < num_vertices; ++i) { + // Ensure normals are normalized. + vertices[i].orientation = normalize(vertices[i].orientation); + + // Inflate the vertex position in direction of normal (if applicable). + if (inflate != 0.0f) { + vertices[i].position += inflate * vertices[i].orientation.xyz; + } + + // Convert the normals into orientations. + vertices[i].orientation = CalculateOrientation(vertices[i].orientation.xyz); + + // Assign uvs (if applicable). + if constexpr (T::kHasUv) { + vertices[i].uv = ReadFloat2(uvs, i); + } + + // Calculate the bounds of the vertex buffer. + *min_pt = min(*min_pt, vertices[i].position); + *max_pt = max(*max_pt, vertices[i].position); + } +} + +static mjrfMeshData PrepareMeshData(int num_vertices, bool has_uvs) { + mjrfMeshData mesh_data; + mjrf_defaultMeshData(&mesh_data); + + if (has_uvs) { + const int nbytes = sizeof(VertexWithUv) * num_vertices; + mesh_data.user_data = new char[nbytes]; + std::memset(mesh_data.user_data, 0, nbytes); + } else { + const int nbytes = sizeof(VertexNoUv) * num_vertices; + mesh_data.user_data = new char[nbytes]; + std::memset(mesh_data.user_data, 0, nbytes); + } + mesh_data.release = [](void* user_data) { + delete[] (char*)(user_data); + }; + + char* buf = reinterpret_cast(mesh_data.user_data); + mesh_data.num_vertices = num_vertices; + mesh_data.num_attributes = has_uvs ? 3 : 2; + mesh_data.interleaved = true; + mesh_data.attributes[0].usage = mjVERTEX_ATTRIBUTE_USAGE_POSITION; + mesh_data.attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; + mesh_data.attributes[0].bytes = buf; + mesh_data.attributes[1].usage = mjVERTEX_ATTRIBUTE_USAGE_TANGENTS; + mesh_data.attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT4; + mesh_data.attributes[1].bytes = buf + sizeof(float[3]); + if (has_uvs) { + mesh_data.attributes[2].usage = mjVERTEX_ATTRIBUTE_USAGE_UV; + mesh_data.attributes[2].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; + mesh_data.attributes[2].bytes = buf + sizeof(float[7]); + } + mesh_data.primitive_type = mjMESH_PRIMITIVE_TYPE_TRIANGLES; + mesh_data.num_indices = num_vertices; + mesh_data.index_type = mjINDEX_TYPE_U32; + mesh_data.indices = nullptr; + return mesh_data; +} + +static void SetBounds(mjrfMeshData* mesh_data, const float3& min_pt, + const float3& max_pt) { + mesh_data->bounds_min[0] = min_pt.x; + mesh_data->bounds_min[1] = min_pt.y; + mesh_data->bounds_min[2] = min_pt.z; + mesh_data->bounds_max[0] = max_pt.x; + mesh_data->bounds_max[1] = max_pt.y; + mesh_data->bounds_max[2] = max_pt.z; +} + +UniquePtr CreateFlexMesh(mjrfContext* ctx, const mjModel* model, + const mjData* data, int flex_id, + int flex_layer, bool smooth_skinning, + bool generate_edges, + bool generate_vertices) { + // TODO: generate edges and vertices if requested. + const int num_vertices = + CalculateVertexCount(model, flex_id, flex_layer, smooth_skinning); + const bool has_uvs = model->flex_texcoordadr[flex_id] >= 0; + mjrfMeshData mesh_data = PrepareMeshData(num_vertices, has_uvs); + + float3 min_pt = float3(FLT_MAX); + float3 max_pt = float3(FLT_MIN); + if (has_uvs) { + VertexWithUv* vertices = (VertexWithUv*)(mesh_data.user_data); + FillFlexVertices(vertices, model, data, flex_id, &min_pt, &max_pt, + flex_layer, smooth_skinning); + } else { + VertexNoUv* vertices = (VertexNoUv*)(mesh_data.user_data); + FillFlexVertices(vertices, model, data, flex_id, &min_pt, &max_pt, + flex_layer, smooth_skinning); + } + SetBounds(&mesh_data, min_pt, max_pt); + return CreateMesh(ctx, mesh_data); +} + +UniquePtr CreateSkinMesh(mjrfContext* ctx, const mjModel* model, + const mjData* data, int skin_id) { + const int num_vertices = model->skin_vertnum[skin_id]; + const bool has_uvs = model->skin_texcoordadr[skin_id] >= 0; + mjrfMeshData mesh_data = PrepareMeshData(num_vertices, has_uvs); + + float3 min_pt = float3(FLT_MAX); + float3 max_pt = float3(FLT_MIN); + if (has_uvs) { + VertexWithUv* vertices = (VertexWithUv*)(mesh_data.user_data); + FillSkinVertices(vertices, model, data, skin_id, &min_pt, &max_pt); + } else { + VertexNoUv* vertices = (VertexNoUv*)(mesh_data.user_data); + FillSkinVertices(vertices, model, data, skin_id, &min_pt, &max_pt); + } + mesh_data.num_indices = 3 * model->skin_facenum[skin_id]; + mesh_data.indices = model->skin_face + 3 * model->skin_faceadr[skin_id]; + SetBounds(&mesh_data, min_pt, max_pt); + return CreateMesh(ctx, mesh_data); +} + +void GatherSpatialTendonPoints(const mjModel* model, const mjData* data, + int tendon_id, std::vector& points) { + mjtNum length = 0.f; + const bool is_catenary = mjv_isCatenary(model, data, tendon_id, &length); + if (is_catenary) { + const int max_segments = mjMIN(model->vis.quality.numslices + 1, 100); + + mjtNum x0[3]; + mju_copy3(x0, data->wrap_xpos + 3 * data->ten_wrapadr[tendon_id] + 0); + + mjtNum x1[3]; + mju_copy3(x1, data->wrap_xpos + 3 * data->ten_wrapadr[tendon_id] + 3); + + const float width = model->tendon_width[tendon_id]; + + mjtNum pts[3 * 100]; + const int npoints = + mjv_catenary(x0, x1, model->opt.gravity, length, pts, max_segments); + + for (int j = 0; j < npoints - 1; ++j) { + points.emplace_back(ReadFloat3(pts, j), width); + points.emplace_back(ReadFloat3(pts, j + 1), width); + } + } else { + const int adr = data->ten_wrapadr[tendon_id]; + const int num = data->ten_wrapnum[tendon_id]; + for (int j = adr; j < adr + num - 1; j++) { + if (data->wrap_obj[j] == -2 || data->wrap_obj[j + 1] == -2) { + continue; + } + + float width = model->tendon_width[tendon_id]; + if (data->wrap_obj[j] >= 0 && data->wrap_obj[j + 1] >= 0) { + width *= 0.5; + } + points.emplace_back(ReadFloat3(data->wrap_xpos, j + 0), width); + points.emplace_back(ReadFloat3(data->wrap_xpos, j + 1), width); + } + } +} +} // namespace mujoco diff --git a/src/render/filament/support/mesh_util.h b/src/render/filament/support/mesh_util.h new file mode 100644 index 00000000..baf97050 --- /dev/null +++ b/src/render/filament/support/mesh_util.h @@ -0,0 +1,45 @@ +// Copyright 2026 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_SRC_RENDER_FILAMENT_SUPPORT_MESH_UTIL_H_ +#define MUJOCO_SRC_RENDER_FILAMENT_SUPPORT_MESH_UTIL_H_ + +#include +#include +#include +#include +#include "render/filament/mjrfilament_cpp.h" + +namespace mujoco { + +// Creates a mjrfMesh for the given flex object. +UniquePtr CreateFlexMesh(mjrfContext* ctx, const mjModel* model, + const mjData* data, int flex_id, + int flex_layer, bool smooth_skinning, + bool generate_edges, bool generate_vertices); + +// Creates a mjrfMesh for the given skin object. +UniquePtr CreateSkinMesh(mjrfContext* ctx, const mjModel* model, + const mjData* data, int skin_id); + +// Populates the set of points that define the given tendon. Points are +// added in pairs, representing the start and end of a segment of the tendon. +// The w-component of the point stores the width/radius of the tendon. +void GatherSpatialTendonPoints(const mjModel* model, const mjData* data, + int tendon_id, + std::vector& points); + +} // namespace mujoco + +#endif // MUJOCO_SRC_RENDER_FILAMENT_SUPPORT_MESH_UTIL_H_ diff --git a/src/render/filament/support/renderable_manager.cc b/src/render/filament/support/renderable_manager.cc new file mode 100644 index 00000000..787821fe --- /dev/null +++ b/src/render/filament/support/renderable_manager.cc @@ -0,0 +1,1009 @@ +// Copyright 2026 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 "render/filament/support/renderable_manager.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "engine/engine_sleep.h" +#include "render/filament/mjrfilament_cpp.h" +#include "render/filament/support/filament_util.h" +#include "render/filament/support/mesh_util.h" +#include "render/filament/support/model_objects.h" + +namespace mujoco { + +using filament::math::float3; +using filament::math::float4; +using filament::math::mat3f; +using filament::math::quatf; + +// Converts an array of numbers into an array of floats. +template +void xtof(float* dst, const T* src, int n) { + for (int i = 0; i < n; ++i) { + dst[i] = static_cast(src[i]); + } +} + +// Returns the tile size for infinite plane texture alignment. +// This is duplicated from engine_vis_visualize.c (re-center infinite plane) +// to ensure UV scaling matches the re-centering increments. +static float GetPlaneTileSize(const mjModel* model, int matid, + float texrepeat) { + if (matid >= 0 && texrepeat > 0) { + return 2.0f / texrepeat; + } else { + const float zfar = model->vis.map.zfar * model->stat.extent; + return 2.1f * zfar / (mjMAXPLANEGRID - 2); + } +} + +static mjtCatBit GetBodyCategory(const mjModel* m, int bodyid) { + if (m->body_weldid[bodyid] == 0 && + m->body_mocapid[m->body_rootid[bodyid]] == -1) { + return mjCAT_STATIC; + } else { + return mjCAT_DYNAMIC; + } +} + +static void Connect(mjrfRenderable* renderable, const float3 p0, + const float3 p1, const float width) { + const float3 vec = p1 - p0; + const float len = length(vec); + const float3 pos = (p0 + p1) * 0.5f; + const float3 size = {width, width, 0.5f * len}; + const quatf rot = + quatf::fromDirectedRotation(normalize(vec), float3{0, 0, 1}); + const mat3f mat = mat3f(rot); + mjrf_setRenderableTransform(renderable, pos.v, mat.asArray()); + mjrf_setRenderableSize(renderable, size.v); +} + +static bool GetSize(const mjModel* model, mjtGeom type, const mjtNum* size, + float out[3]) { + out[0] = size[0]; + out[1] = size[1]; + out[2] = size[2]; + switch (type) { + case mjGEOM_SPHERE: + out[2] = size[0]; + out[1] = size[0]; + return true; + case mjGEOM_CAPSULE: + case mjGEOM_CYLINDER: + out[2] = size[1]; + out[1] = size[0]; + return true; + case mjGEOM_TRIANGLE: + case mjGEOM_PLANE: + out[2] = 1.0f; + return true; + case mjGEOM_ELLIPSOID: + case mjGEOM_BOX: + case mjGEOM_ARROW: + case mjGEOM_ARROW1: + case mjGEOM_ARROW2: + case mjGEOM_LINE: + case mjGEOM_LINEBOX: + return true; + case mjGEOM_MESH: + case mjGEOM_SDF: + case mjGEOM_HFIELD: + case mjGEOM_FLEX: + case mjGEOM_SKIN: + case mjGEOM_NONE: + case mjGEOM_LABEL: + return false; + case mjNGEOMTYPES: + mju_warning("Unsupported geom type: %d", type); + return false; + } +} + +static void SetGeomMesh(mjrfRenderable* renderable, ModelObjects* model_objs, + mjtGeom type, int geom_index = -1) { + const mjModel* model = model_objs->GetModel(); + const int nstack = model->vis.quality.numstacks; + const int nslice = model->vis.quality.numslices; + const int nquad = model->vis.quality.numquads; + + switch (type) { + case mjGEOM_MESH: + case mjGEOM_SDF: { + const int data_id = model->geom_dataid[geom_index] * 2; + mjrf_setRenderableMesh(renderable, model_objs->GetMesh(data_id), 0, 0); + break; + } + case mjGEOM_HFIELD: { + const int data_id = model->geom_dataid[geom_index]; + mjrf_setRenderableMesh(renderable, model_objs->GetHeightField(data_id), 0, 0); + break; + } + case mjGEOM_FLEX: + mju_error("Flex meshes should be handled separately."); + break; + case mjGEOM_SKIN: + mju_error("Skin meshes should be handled separately."); + break; + case mjGEOM_PLANE: + case mjGEOM_SPHERE: + case mjGEOM_ELLIPSOID: + case mjGEOM_BOX: + case mjGEOM_CAPSULE: + case mjGEOM_CYLINDER: + case mjGEOM_ARROW: + case mjGEOM_ARROW1: + case mjGEOM_ARROW2: + case mjGEOM_LINE: + case mjGEOM_LINEBOX: + case mjGEOM_TRIANGLE: + mjrf_setRenderableGeomMesh(renderable, type, nstack, nslice, nquad); + break; + case mjGEOM_NONE: + case mjGEOM_LABEL: + // Do nothing. + break; + case mjNGEOMTYPES: + mju_warning("Unsupported geom type: %d", type); + break; + } +} + +RenderableManager::RenderableManager(mjrfContext* ctx, mjrfScene* scene, + ModelObjects* model_objects) + : ctx_(ctx), scene_(scene), model_objects_(model_objects) { + mjv_defaultOption(&vopts_); + + AddGeomGeoms(); + AddSiteGeoms(); + AddFlexGeoms(); + AddSkinGeoms(); + AddSliderCrankGeoms(); + // A tendon is composed of a collection of renderables which are managed + // every frame. For now, all we do is reserve space for the collections. + tendons_.resize(model_objects_->GetModel()->ntendon); +} + +RenderableManager::~RenderableManager() { + for (auto& renderable : geoms_) { + mjrf_removeRenderableFromScene(scene_, renderable.get()); + } + for (auto& renderable : sites_) { + mjrf_removeRenderableFromScene(scene_, renderable.get()); + } + for (auto& renderable : flexes_) { + mjrf_removeRenderableFromScene(scene_, renderable.get()); + } + for (auto& renderable : skins_) { + mjrf_removeRenderableFromScene(scene_, renderable.get()); + } + for (auto& segments : tendons_) { + for (auto& renderable : segments) { + mjrf_removeRenderableFromScene(scene_, renderable.get()); + } + } + for (auto& renderable : sliders_) { + mjrf_removeRenderableFromScene(scene_, renderable.get()); + } + for (auto& renderable : cranks_) { + mjrf_removeRenderableFromScene(scene_, renderable.get()); + } +} + +void RenderableManager::Update(const mjData* data) { + const mjModel* model = model_objects_->GetModel(); + + for (int i = 0; i < model->ngeom; ++i) { + const float3 pos = ReadFloat3(data->geom_xpos, i); + const mat3f mat = ReadMat3(data->geom_xmat, i); + mjrf_setRenderableTransform(geoms_[i].get(), pos.v, mat.asArray()); + } + + for (int i = 0; i < model->nsite; ++i) { + const float3 pos = ReadFloat3(data->site_xpos, i); + const mat3f mat = ReadMat3(data->site_xmat, i); + mjrf_setRenderableTransform(sites_[i].get(), pos.v, mat.asArray()); + } + + + for (int i = 0; i < model->nflex; ++i) { + const int flex_layer = vopts_.flex_layer; + const bool smooth_skinning = vopts_.flags[mjVIS_FLEXSKIN]; + const bool edges = !smooth_skinning && vopts_.flags[mjVIS_FLEXEDGE]; + const bool vertices = !smooth_skinning && vopts_.flags[mjVIS_FLEXVERT]; + auto mesh = CreateFlexMesh(ctx_, model, data, i, flex_layer, + smooth_skinning, edges, vertices); + mjrf_setRenderableMesh(flexes_[i].get(), mesh.get(), 0, 0); + flex_meshes_[i] = std::move(mesh); + } + + for (int i = 0; i < model->nskin; ++i) { + auto mesh = CreateSkinMesh(ctx_, model, data, i); + mjrf_setRenderableMesh(skins_[i].get(), mesh.get(), 0, 0); + skin_meshes_[i] = std::move(mesh); + } + + for (int i = 0; i < model->ntendon; i++) { + UpdateSpatialTendons(data, i); + } + + int renderable_index = 0; + for (int i = 0; i < model->nu; i++) { + if (model->actuator_trntype[i] != mjTRN_SLIDERCRANK) { + continue; + } + UpdateSliderCranks(data, i, renderable_index++); + } + + if (vopts_.flags[mjVIS_ISLAND]) { + const bool sleep_enabled = model->opt.enableflags & mjENBL_SLEEP; + + for (int i = 0; i < model->ngeom; ++i) { + const int weld_id = model->body_weldid[model->geom_bodyid[i]]; + if (!model->body_dofnum[weld_id]) { + continue; + } + + const int awake = data->body_awake[model->geom_bodyid[i]]; + const int dof = model->body_dofadr[weld_id]; + const int island = data->nisland ? data->dof_island[dof] : -1; + int island_id = island >= 0 ? data->island_dofadr[island] : -1; + if (island_id == -1 && sleep_enabled) { + int tree = model->dof_treeid[dof]; + if (!awake) { + tree = mj_sleepCycle(data->tree_asleep, model->ntree, tree); + } + island_id = model->tree_dofadr[tree]; + } + + mjrfMaterial material; + mjrf_getRenderableMaterial(geoms_[i].get(), &material); + material.island_id = island_id; + material.sleep_state = awake ? mjS_AWAKE : mjS_ASLEEP; + mjrf_setRenderableMaterial(geoms_[i].get(), &material); + } + for (int i = 0; i < model->nflex; ++i) { + int bodyid = -1; + if (model->flex_interp[i]) { + int nodeadr = model->flex_nodeadr[i]; + for (int j = 0; j < model->flex_nodenum[i] && bodyid < 0; j++) { + int b = model->flex_nodebodyid[nodeadr+j]; + if (model->body_treeid[b] >= 0) bodyid = b; + } + } else { + int vertadr = model->flex_vertadr[i]; + for (int j=0; j < model->flex_vertnum[i] && bodyid < 0; j++) { + int b = model->flex_vertbodyid[vertadr+j]; + if (model->body_treeid[b] >= 0) bodyid = b; + } + } + if (bodyid < 0) { + continue; + } + int weld_id = model->body_weldid[bodyid]; + int dof = model->body_dofadr[weld_id]; + int island = data->nisland ? data->dof_island[dof] : -1; + int island_id = island >= 0 ? data->island_dofadr[island] : -1; + int awake = data->body_awake[bodyid]; + if (island_id == -1 && sleep_enabled) { + int tree = model->dof_treeid[dof]; + if (!awake) { + tree = mj_sleepCycle(data->tree_asleep, model->ntree, tree); + } + island_id = model->tree_dofadr[tree]; + } + + mjrfMaterial material; + mjrf_getRenderableMaterial(flexes_[i].get(), &material); + material.island_id = island_id; + material.sleep_state = awake ? mjS_AWAKE : mjS_ASLEEP; + mjrf_setRenderableMaterial(flexes_[i].get(), &material); + } + } +} + +mjrfRenderable* RenderableManager::GetRenderable(mjtObj obj_type, + int obj_index) { + switch (obj_type) { + case mjOBJ_GEOM: + if (obj_index >= 0 && obj_index < geoms_.size()) { + return geoms_[obj_index].get(); + } + break; + case mjOBJ_SITE: + if (obj_index >= 0 && obj_index < sites_.size()) { + return sites_[obj_index].get(); + } + break; + case mjOBJ_FLEX: + if (obj_index >= 0 && obj_index < flexes_.size()) { + return flexes_[obj_index].get(); + } + break; + case mjOBJ_SKIN: + if (obj_index >= 0 && obj_index < skins_.size()) { + return skins_[obj_index].get(); + } + break; + case mjOBJ_ACTUATOR: + if (obj_index >= 0 && obj_index < sliders_.size()) { + return sliders_[obj_index].get(); + } + break; + default: + break; + } + return nullptr; +} + +void RenderableManager::AddGeomGeoms() { + const mjModel* model = model_objects_->GetModel(); + + geoms_.reserve(model->ngeom); + for (int i = 0; i < model->ngeom; ++i) { + const mjtGeom type = (mjtGeom)model->geom_type[i]; + + mjrfRenderableParams params; + mjrf_defaultRenderableParams(¶ms); + auto renderable = CreateRenderable(ctx_, params); + + SetGeomMesh(renderable.get(), model_objects_, type, i); + + mjrfMaterial material = GetDefaultMaterial(mjOBJ_GEOM, i); + mjrf_setRenderableMaterial(renderable.get(), &material); + + float size[3]; + if (GetSize(model, type, model->geom_size + (3 * i), size)) { + mjrf_setRenderableSize(renderable.get(), size); + } + + if (vopts_.geomgroup[model->geom_group[i]]) { + mjrf_addRenderableToScene(scene_, renderable.get()); + } + geoms_.emplace_back(std::move(renderable)); + } +} + +void RenderableManager::AddSiteGeoms() { + const mjModel* model = model_objects_->GetModel(); + + sites_.reserve(model->nsite); + for (int i = 0; i < model->nsite; ++i) { + const mjtGeom type = (mjtGeom)model->site_type[i]; + + mjrfRenderableParams params; + mjrf_defaultRenderableParams(¶ms); + auto renderable = CreateRenderable(ctx_, params); + + SetGeomMesh(renderable.get(), model_objects_, type); + + mjrfMaterial material = GetDefaultMaterial(mjOBJ_SITE, i); + mjrf_setRenderableMaterial(renderable.get(), &material); + + float size[3]; + if (GetSize(model, type, model->site_size + (3 * i), size)) { + mjrf_setRenderableSize(renderable.get(), size); + } + + if (vopts_.sitegroup[model->site_group[i]]) { + mjrf_addRenderableToScene(scene_, renderable.get()); + } + sites_.push_back(std::move(renderable)); + } +} + +void RenderableManager::AddFlexGeoms() { + const mjModel* model = model_objects_->GetModel(); + + flexes_.reserve(model->nflex); + flex_meshes_.reserve(model->nflex); + for (int i = 0; i < model->nflex; ++i) { + mjrfRenderableParams params; + mjrf_defaultRenderableParams(¶ms); + auto renderable = CreateRenderable(ctx_, params); + + mjrfMaterial material = GetDefaultMaterial(mjOBJ_FLEX, i); + mjrf_setRenderableMaterial(renderable.get(), &material); + + if (vopts_.flexgroup[model->flex_group[i]]) { + mjrf_addRenderableToScene(scene_, renderable.get()); + } + flexes_.emplace_back(std::move(renderable)); + flex_meshes_.emplace_back(nullptr, nullptr); + } +} + +void RenderableManager::AddSkinGeoms() { + const mjModel* model = model_objects_->GetModel(); + + skins_.reserve(model->nskin); + skin_meshes_.reserve(model->nskin); + for (int i = 0; i < model->nskin; i++) { + mjrfRenderableParams params; + mjrf_defaultRenderableParams(¶ms); + auto renderable = CreateRenderable(ctx_, params); + + mjrfMaterial material = GetDefaultMaterial(mjOBJ_SKIN, i); + mjrf_setRenderableMaterial(renderable.get(), &material); + + if (vopts_.skingroup[model->skin_group[i]]) { + mjrf_addRenderableToScene(scene_, renderable.get()); + } + skins_.emplace_back(std::move(renderable)); + skin_meshes_.emplace_back(nullptr, nullptr); + } +} + +void RenderableManager::AddSliderCrankGeoms() { + const mjModel* model = model_objects_->GetModel(); + const int nstack = model->vis.quality.numstacks; + const int nslice = model->vis.quality.numslices; + const int nquad = model->vis.quality.numquads; + + mjrfRenderableParams params; + mjrf_defaultRenderableParams(¶ms); + + sliders_.reserve(model->nu); + cranks_.reserve(model->nu); + for (int i = 0; i < model->nu; i++) { + if (model->actuator_trntype[i] != mjTRN_SLIDERCRANK) { + continue; + } + + // Create two renderables, one for the slider and the other for the crank. + auto slider = CreateRenderable(ctx_, params); + mjrf_setRenderableGeomMesh(slider.get(), mjGEOM_CYLINDER, nstack, nslice, + nquad); + mjrf_addRenderableToScene(scene_, slider.get()); + sliders_.emplace_back(std::move(slider)); + + auto crank = CreateRenderable(ctx_, params); + mjrf_setRenderableGeomMesh(crank.get(), mjGEOM_CAPSULE, nstack, nslice, + nquad); + mjrf_addRenderableToScene(scene_, crank.get()); + cranks_.emplace_back(std::move(crank)); + } +} + +void RenderableManager::UpdateSliderCranks(const mjData* data, int actuator_id, + int index) { + const mjModel* model = model_objects_->GetModel(); + const float scale = model->stat.meansize; + const float slider_width = scale * model->vis.scale.slidercrank; + const float crank_width = scale * model->vis.scale.slidercrank / 2.0; + + const int crank_id = model->actuator_trnid[2 * actuator_id]; + const int slider_id = model->actuator_trnid[2 * actuator_id + 1]; + const float3 crank_pos = ReadFloat3(data->site_xpos, crank_id); + const float3 slider_pos = ReadFloat3(data->site_xpos, slider_id); + + const float3 vec = crank_pos - slider_pos; + + float3 axis; + axis[0] = data->site_xmat[9 * slider_id + 2]; + axis[1] = data->site_xmat[9 * slider_id + 5]; + axis[2] = data->site_xmat[9 * slider_id + 8]; + float len = dot(vec, axis); + + const float rod = model->actuator_cranklength[actuator_id]; + const float det = (len * len) + (rod * rod) - dot(vec, vec); + const bool broken = (det < 0); + if (det > 0) { + len -= std::sqrt(det); + } + + const float3 end = slider_pos + (axis * len); + + mjrfRenderable* slider = sliders_[index].get(); + mjrfRenderable* crank = cranks_[index].get(); + Connect(slider, slider_pos, end, slider_width); + Connect(crank, end, crank_pos, crank_width); + + mjrfMaterial material = GetDefaultMaterial(mjOBJ_ACTUATOR, actuator_id); + + xtof(material.color, model->vis.rgba.slidercrank, 4); + mjrf_setRenderableMaterial(slider, &material); + + if (broken) { + xtof(material.color, model->vis.rgba.crankbroken, 4); + } + mjrf_setRenderableMaterial(crank, &material); +} + +void RenderableManager::AppendSegmentToTendon(int tendon_id) { + const mjModel* model = model_objects_->GetModel(); + const int nstack = model->vis.quality.numstacks; + const int nslice = model->vis.quality.numslices; + const int nquad = model->vis.quality.numquads; + mjrfRenderableParams params; + mjrf_defaultRenderableParams(¶ms); + auto renderable = CreateRenderable(ctx_, params); + mjrf_setRenderableGeomMesh(renderable.get(), mjGEOM_CAPSULE, nstack, nslice, + nquad); + if (vopts_.tendongroup[model->tendon_group[tendon_id]]) { + mjrf_addRenderableToScene(scene_, renderable.get()); + } + + auto& segments = tendons_[tendon_id]; + segments.emplace_back(std::move(renderable)); +} + +void RenderableManager::RemoveSegmentFromTendon(int tendon_id) { + auto& segments = tendons_[tendon_id]; + mjrfRenderable* renderable = segments.back().get(); + mjrf_removeRenderableFromScene(scene_, renderable); + segments.pop_back(); +} + +void RenderableManager::UpdateSpatialTendons(const mjData* data, int tendon_id) { + const mjModel* model = model_objects_->GetModel(); + + // Gather the points that define the tendon. We'll use a simple cache to avoid + // excessive reallocations. + point_cache_.clear(); + GatherSpatialTendonPoints(model, data, tendon_id, point_cache_); + + // Tendons are composed on N "segments" represented by cylinder geoms. + // We adjust the number of segments we need to match the number of points. + const int nsegments = static_cast(point_cache_.size()) / 2; + std::vector>& segments = tendons_[tendon_id]; + while (tendons_[tendon_id].size() < nsegments) { + AppendSegmentToTendon(tendon_id); + } + while (tendons_[tendon_id].size() > nsegments) { + RemoveSegmentFromTendon(tendon_id); + } + + mjrfMaterial material = GetDefaultMaterial(mjOBJ_TENDON, tendon_id); + + // If tendon has no explicit color then color it using limit impedance. + if (model->tendon_matid[tendon_id] == -1 && material.color[0] == 0.5 && + material.color[1] == 0.5 && material.color[2] == 0.5 && + material.color[3] == 1) { + mjtNum imp = 0; + int efc_start = data->ne + data->nf; + int efc_end = efc_start + data->nl; + for (int k = efc_start; k < efc_end; k++) { + if (data->efc_type[k] == mjCNSTR_LIMIT_TENDON && + data->efc_id[k] == tendon_id) { + imp = data->efc_KBIP[4 * k + 2]; + } + } + const float scale = (1 - imp); + const float* constraint = model->vis.rgba.constraint; + material.color[0] = scale * material.color[0] + imp * constraint[0]; + material.color[1] = scale * material.color[1] + imp * constraint[1]; + material.color[2] = scale * material.color[2] + imp * constraint[2]; + } + + if (vopts_.flags[mjVIS_ISLAND]) { + const int ecf = data->tendon_efcadr[tendon_id]; + if (data->nisland && ecf >= 0) { + material.island_id = data->island_dofadr[data->efc_island[ecf]]; + material.sleep_state = mjS_AWAKE; + } + } + + for (int i = 0; i < point_cache_.size(); i += 2) { + mjrfRenderable* renderable = segments[i / 2].get(); + const float width = point_cache_[i].w; + const float3 p0 = point_cache_[i + 0].xyz; + const float3 p1 = point_cache_[i + 1].xyz; + Connect(renderable, p0, p1, width); + mjrf_setRenderableMaterial(renderable, &material); + } +} + +int RenderableManager::GetSegmentationId(mjtObj obj_type, int obj_index) { + const mjModel* model = model_objects_->GetModel(); + + int id = 0; + if (obj_type == mjOBJ_GEOM) { + return id + obj_index; + } else { + id += model->ngeom; + } + if (obj_type == mjOBJ_SITE) { + return id + obj_index; + } else { + id += model->nsite; + } + if (obj_type == mjOBJ_FLEX) { + return id + obj_index; + } else { + id += model->nflex; + } + if (obj_type == mjOBJ_SKIN) { + return id + obj_index; + } else { + id += model->nskin; + } + if (obj_type == mjOBJ_TENDON) { + return id + obj_index; + } else { + id += model->ntendon; + } + if (obj_type == mjOBJ_ACTUATOR) { + return id + obj_index; + } else { + id += model->nu; + } + mju_error("Unsupported object type: %d", obj_type); + return -1; +} + +mjrfMaterial RenderableManager::GetDefaultMaterial(mjtObj obj_type, + int obj_index) { + const mjModel* model = model_objects_->GetModel(); + + mjrfMaterial material; + mjrf_defaultMaterial(&material); + + int matid = -1; + mjtGeom geom_type = mjGEOM_NONE; + const float* rgba = nullptr; + const mjtNum* size = nullptr; + + switch (obj_type) { + case mjOBJ_GEOM: + geom_type = (mjtGeom)model->geom_type[obj_index]; + rgba = model->geom_rgba + (4 * obj_index); + size = model->geom_size + (3 * obj_index); + matid = model->geom_matid[obj_index]; + break; + case mjOBJ_SITE: + geom_type = (mjtGeom)model->site_type[obj_index]; + rgba = model->site_rgba + (4 * obj_index); + size = model->site_size + (3 * obj_index); + matid = model->site_matid[obj_index]; + break; + case mjOBJ_FLEX: + geom_type = mjGEOM_FLEX; + rgba = model->flex_rgba + (4 * obj_index); + size = model->flex_size + (3 * obj_index); + matid = model->flex_matid[obj_index]; + break; + case mjOBJ_SKIN: + geom_type = mjGEOM_SKIN; + rgba = model->skin_rgba + (4 * obj_index); + matid = model->skin_matid[obj_index]; + break; + case mjOBJ_TENDON: + geom_type = mjGEOM_CAPSULE; + rgba = model->tendon_rgba + (4 * obj_index); + size = model->tendon_width + obj_index; + matid = model->tendon_matid[obj_index]; + break; + case mjOBJ_ACTUATOR: + geom_type = mjGEOM_CYLINDER; + rgba = model->vis.rgba.slidercrank; + // size = model->tendon_width + obj_index; + break; + default: + mju_error("Unsupported object type: %d", obj_type); + break; + } + + xtof(material.color, rgba, 4); + + if (matid >= 0 && matid < model->nmat) { + auto get_texture = [&](int role) -> const mjrfTexture* { + const int tex_id = model->mat_texid[matid * mjNTEXROLE + role]; + return tex_id >= 0 ? model_objects_->GetTexture(tex_id) : nullptr; + }; + material.color_texture = get_texture(mjTEXROLE_RGB); + material.normal_texture = get_texture(mjTEXROLE_NORMAL); + material.emissive_texture = get_texture(mjTEXROLE_EMISSIVE); + material.orm_texture = get_texture(mjTEXROLE_ORM); + material.metallic_texture = get_texture(mjTEXROLE_METALLIC); + material.roughness_texture = get_texture(mjTEXROLE_ROUGHNESS); + material.occlusion_texture = get_texture(mjTEXROLE_OCCLUSION); + + material.emissive = model->mat_emission[matid]; + material.specular = model->mat_specular[matid]; + material.glossiness = model->mat_shininess[matid]; + material.reflectance = model->mat_reflectance[matid]; + material.metallic = model->mat_metallic[matid]; + material.roughness = model->mat_roughness[matid]; + + const float* rgba = model->mat_rgba + 4 * matid; + if (rgba[0] != 0.5f || rgba[1] != 0.5f || rgba[2] != 0.5f || + rgba[3] != 1.0f) { + xtof(material.color, rgba, 4); + } + } else { + material.emissive = 0; + material.specular = 0.5; + material.glossiness = 0.5; + material.reflectance = 0; + } + + // UvScale only applies to objects that don't have explicit UV coordinates + // in their vertex buffer. Instead, we set the UV coordinate to be the same + // as the vertex position. + // + // The material's `texuniform` and `texrepeat` parameters allow us to scale + // the programmatic UVs. + + if (material.color_texture) { + float fsize[3] = {1.0f, 1.0f, 1.0f}; + if (size) { + xtof(fsize, size, 3); + if (geom_type != mjGEOM_NONE) { + switch (geom_type) { + case mjGEOM_SPHERE: + fsize[2] = size[0]; + fsize[1] = size[0]; + break; + case mjGEOM_CAPSULE: + case mjGEOM_CYLINDER: + fsize[2] = size[1]; + fsize[1] = size[0]; + break; + case mjGEOM_TRIANGLE: + case mjGEOM_PLANE: + fsize[2] = 1.0f; + break; + default: + break; + } + } + } + + const bool tex_uniform = model->mat_texuniform[matid]; + if (mjrf_getTextureSamplerType(material.color_texture) == mjTEXTURE_2D) { + // For 2D textures, `tex_repeat` specifies how many times the texture + // image is repeated. The `tex_uniform` flag determines if the repetition + // is applied at in object space (false) or in world space (true). + float tex_repeat[2]; + tex_repeat[0] = model->mat_texrepeat[(matid * 2) + 0]; + tex_repeat[1] = model->mat_texrepeat[(matid * 2) + 1]; + material.uv_scale[0] = tex_repeat[0]; + material.uv_scale[1] = tex_repeat[1]; + + if (geom_type == mjGEOM_MESH || geom_type == mjGEOM_HFIELD || + geom_type == mjGEOM_SDF) { + if (fsize[0] > mjMINVAL) { + material.uv_scale[0] /= fsize[0]; + } + if (fsize[1] > mjMINVAL) { + material.uv_scale[1] /= fsize[1]; + } + } + + if (tex_uniform) { + if (fsize[0] > 0) { + material.uv_scale[0] *= fsize[0]; + } + if (fsize[1] > 0) { + material.uv_scale[1] *= fsize[1]; + } + } + const bool is_infinite_plane = + geom_type == mjGEOM_PLANE && (fsize[0] <= 0 || fsize[1] <= 0); + if (is_infinite_plane) { + // Infinite planes are scaled to match the tile size used by + // re-centering in engine_vis_visualize.c. + const float plane_scale = static_cast(mjMAXPLANEGRID) / 2.0f; + const float tile_size_x = GetPlaneTileSize(model, matid, tex_repeat[0]); + const float tile_size_y = GetPlaneTileSize(model, matid, tex_repeat[1]); + material.uv_scale[0] = 2.0f * plane_scale / tile_size_x; + material.uv_scale[1] = 2.0f * plane_scale / tile_size_y; + } + + // We want to do the equivalent of: + // mjr_setf4(splane, 0.5 * scl.x, 0, 0, -0.5); + // mjr_setf4(tplane, 0, -0.5 * scl.y, 0, -0.5); + // glTexGenfv(GL_S, GL_OBJECT_PLANE, splane); + // glTexGenfv(GL_T, GL_OBJECT_PLANE, tplane); + material.uv_scale[0] = 0.5f * material.uv_scale[0]; + material.uv_scale[1] = -0.5f * material.uv_scale[1]; + material.uv_offset[0] = -0.5f; + material.uv_offset[1] = -0.5f; + } else { + // For cube maps, if `tex_uniform` is true, then scale the texture so that + // it covers a 1x1 area of world space rather than the area of the object. + if (tex_uniform) { + material.uv_scale[0] = 1.0f / (fsize[0] ? fsize[0] : 1.0f); + material.uv_scale[1] = 1.0f / (fsize[1] ? fsize[1] : 1.0f); + material.uv_scale[2] = 1.0f / (fsize[2] ? fsize[2] : 1.0f); + } + } + } + + // Apply material multipliers from the model. + material.emissive *= model_objects_->GetEmissiveMultiplier(); + material.specular *= model_objects_->GetSpecularMultiplier(); + material.glossiness *= model_objects_->GetShininessMultiplier(); + + material.segmentation_id = GetSegmentationId(obj_type, obj_index); + + return material; +} + +void RenderableManager::SelectObject(mjtObj obj_type, int obj_index) { + if (obj_type != selected_obj_type_ || obj_index != selected_obj_index_) { + mjrfMaterial material; + + mjrfRenderable* prev_renderable = GetSelectedRenderable(); + if (prev_renderable) { + mjrf_getRenderableMaterial(prev_renderable, &material); + material.selected = 0; + mjrf_setRenderableMaterial(prev_renderable, &material); + } + + selected_obj_type_ = obj_type; + selected_obj_index_ = obj_index; + + mjrfRenderable* curr_renderable = GetSelectedRenderable(); + if (curr_renderable) { + mjrf_getRenderableMaterial(curr_renderable, &material); + material.selected = 1; + mjrf_setRenderableMaterial(curr_renderable, &material); + } + } +} + +mjrfRenderable* RenderableManager::GetSelectedRenderable() { + const mjModel* model = model_objects_->GetModel(); + if (selected_obj_type_ == mjOBJ_FLEX) { + return flexes_[selected_obj_index_].get(); + } else if (selected_obj_type_ == mjOBJ_SKIN) { + return skins_[selected_obj_index_].get(); + } else if (selected_obj_type_ == mjOBJ_BODY) { + for (int i = 0; i < model->ngeom; ++i) { + if (model->geom_bodyid[i] == selected_obj_index_) { + return geoms_[i].get(); + } + } + for (int i = 0; i < model->nsite; ++i) { + if (model->site_bodyid[i] == selected_obj_index_) { + return sites_[i].get(); + } + } + } + return nullptr; +} + +void RenderableManager::SetVisibility(mjtObj obj_type, bool visible, + std::optional group) { + const mjModel* model = model_objects_->GetModel(); + + switch (obj_type) { + case mjOBJ_GEOM: + if (group.has_value()) { + if (vopts_.geomgroup[group.value()] == visible) { + return; + } + for (int i = 0; i < model->ngeom; ++i) { + if (model->geom_group[i] != group.value()) { + continue; + } + mjrfRenderable* renderable = geoms_[i].get(); + if (visible) { + mjrf_addRenderableToScene(scene_, renderable); + } else { + mjrf_removeRenderableFromScene(scene_, renderable); + } + } + } else { + mju_error("Unsupported object type: %d", obj_type); + } + break; + case mjOBJ_SITE: + if (group.has_value()) { + if (vopts_.sitegroup[group.value()] == visible) { + return; + } + for (int i = 0; i < model->nsite; ++i) { + if (model->site_group[i] != group.value()) { + continue; + } + mjrfRenderable* renderable = sites_[i].get(); + if (visible) { + mjrf_addRenderableToScene(scene_, renderable); + } else { + mjrf_removeRenderableFromScene(scene_, renderable); + } + } + } else { + mju_error("Unsupported object type: %d", obj_type); + } + break; + case mjOBJ_JOINT: + break; + case mjOBJ_TENDON: + break; + case mjOBJ_ACTUATOR: + break; + case mjOBJ_FLEX: + break; + case mjOBJ_SKIN: + break; + default: + mju_error("Unsupported object type: %d", obj_type); + break; + } +} + +void RenderableManager::Apply(const mjvOption& vopts) { + const mjModel* model = model_objects_->GetModel(); + for (int i = 0; i < mjNGROUP; ++i) { + SetVisibility(mjOBJ_GEOM, vopts.geomgroup[i], i); + SetVisibility(mjOBJ_SITE, vopts.sitegroup[i], i); + SetVisibility(mjOBJ_JOINT, vopts.jointgroup[i], i); + SetVisibility(mjOBJ_TENDON, vopts.tendongroup[i], i); + SetVisibility(mjOBJ_ACTUATOR, vopts.actuatorgroup[i], i); + SetVisibility(mjOBJ_FLEX, vopts.flexgroup[i], i); + SetVisibility(mjOBJ_SKIN, vopts.skingroup[i], i); + } + SetVisibility(mjOBJ_JOINT, vopts.flags[mjVIS_JOINT]); + SetVisibility(mjOBJ_TENDON, vopts.flags[mjVIS_TENDON]); + SetVisibility(mjOBJ_ACTUATOR, vopts.flags[mjVIS_ACTUATOR]); + SetVisibility(mjOBJ_SKIN, vopts.flags[mjVIS_SKIN]); + SetVisibility(mjOBJ_FLEX, vopts.flags[mjVIS_FLEXSKIN]); + + // Swap geoms between convex and non-convex meshes based on flags. + if (vopts.flags[mjVIS_CONVEXHULL] != vopts_.flags[mjVIS_CONVEXHULL]) { + for (int i = 0; i < model->ngeom; ++i) { + const mjtGeom geom_type = (mjtGeom)model->geom_type[i]; + if (geom_type == mjGEOM_MESH || geom_type == mjGEOM_SDF) { + mjrfRenderable* renderable = geoms_[i].get(); + const int mesh_id = + model->geom_dataid[i] * 2 + (vopts.flags[mjVIS_CONVEXHULL] ? 1 : 0); + const mjrfMesh* mesh = model_objects_->GetMesh(mesh_id); + if (mesh) { + mjrf_setRenderableMesh(renderable, mesh, 0, 0); + } + } + } + } + + // Adjust alpha of dynamic geoms based on transparent flag. + if (vopts.flags[mjVIS_TRANSPARENT] != vopts_.flags[mjVIS_TRANSPARENT]) { + float multiplier = model->vis.map.alpha; + if (!vopts.flags[mjVIS_TRANSPARENT]) { + multiplier = 1.0f / multiplier; + } + + for (int i = 0; i < model->ngeom; ++i) { + const int category = GetBodyCategory(model, model->geom_bodyid[i]); + if (category == mjCAT_DYNAMIC) { + mjrfMaterial material; + mjrf_getRenderableMaterial(geoms_[i].get(), &material); + material.color[3] *= multiplier; + mjrf_setRenderableMaterial(geoms_[i].get(), &material); + } + } + } + + vopts_ = vopts; +} + +} // namespace mujoco diff --git a/src/render/filament/support/renderable_manager.h b/src/render/filament/support/renderable_manager.h new file mode 100644 index 00000000..c1be1606 --- /dev/null +++ b/src/render/filament/support/renderable_manager.h @@ -0,0 +1,100 @@ +// Copyright 2026 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_SRC_EXPERIMENTAL_FILAMENT_COMPAT_RENDERABLE_MANAGER_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_RENDERABLE_MANAGER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include "render/filament/mjrfilament_cpp.h" +#include "render/filament/support/model_objects.h" + +namespace mujoco { + +// Manages Renderable entities for an mjrfScene. +class RenderableManager { + public: + // Populates the mjrScene with the renderables from the model. + RenderableManager(mjrfContext* ctx, mjrfScene* scene, + ModelObjects* model_objects); + ~RenderableManager(); + + // Updates the state of the renderables in the scene. + void Update(const mjData* data); + + // Returns the renderable corresponding to the given model object. + mjrfRenderable* GetRenderable(mjtObj obj_type, int obj_index); + + // Returns the default material (as defined in the mjModel) for the given + // object. Useful if you want to "reset" the material of a renderable back + // to its default. + mjrfMaterial GetDefaultMaterial(mjtObj obj_type, int obj_index); + + // Marks the given object as "selected", unmarking any previously selected + // object. + void SelectObject(mjtObj obj_type, int obj_index); + + // Sets the visibility of all renderables of the given type. If `group` is + // specified, only applies to renderables in that group. + void SetVisibility(mjtObj obj_type, bool visible, + std::optional group = std::nullopt); + + // Applies the visualization options to the renderables in the scene. + void Apply(const mjvOption& vopts); + + private: + void AddGeomGeoms(); + void AddSiteGeoms(); + void AddFlexGeoms(); + void AddSkinGeoms(); + void AddSliderCrankGeoms(); + + void UpdateSpatialTendons(const mjData* data, int tendon_id); + void AppendSegmentToTendon(int tendon_id); + void RemoveSegmentFromTendon(int tendon_id); + void UpdateSliderCranks(const mjData* data, int actuator_id, int index); + + int GetSegmentationId(mjtObj obj_type, int obj_index); + + mjrfRenderable* GetSelectedRenderable(); + + mjrfContext* ctx_; + mjrfScene* scene_; + ModelObjects* model_objects_; + + mjvOption vopts_; + + std::vector> geoms_; + std::vector> sites_; + std::vector> flexes_; + std::vector> skins_; + std::vector> sliders_; + std::vector> cranks_; + std::vector>> tendons_; + + std::vector> flex_meshes_; + std::vector> skin_meshes_; + std::vector point_cache_; + + mjtObj selected_obj_type_ = mjOBJ_UNKNOWN; + int selected_obj_index_ = -1; +}; +} // namespace mujoco + +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_RENDERABLE_MANAGER_H_