Correctly compute bounding boxes for meshes.

Enable culling when bounding box is available.

PiperOrigin-RevId: 892289875
Change-Id: I124426b6e7e72f4d9a1486320d72d6eb032e0828

src/experimental/filament/filament/buffer_util.h

@@ -18,6 +18,7 @@
 #include <cstddef>
 #include <cstdint>
 #include <functional>
+#include <optional>
 #include <type_traits>
 #include <backend/BufferDescriptor.h>
 #include <filament/Box.h>
@@ -33,7 +34,7 @@ namespace mujoco {
 struct FilamentBuffers {
   filament::IndexBuffer* index_buffer = nullptr;
   filament::VertexBuffer* vertex_buffer = nullptr;
-  filament::Box bounds = {{-1, -1, -1}, {1, 1, 1}};
+  std::optional<filament::Box> bounds = std::nullopt;
   filament::RenderableManager::PrimitiveType type =
       filament::RenderableManager::PrimitiveType::TRIANGLES;
 };

src/experimental/filament/filament/builtins.cc

@@ -87,7 +87,7 @@ class LineBuilder {
   }
 
   filament::Box GetBounds() const {
-    return {{-0.001, -0.001, 0}, {0.001, 0.001, 1}};
+    return filament::Box().set({-0.001, -0.001, 0}, {0.001, 0.001, 1});
   }
 };
 
@@ -137,7 +137,9 @@ class PlaneBuilder {
     }
   }
 
-  filament::Box GetBounds() const { return {{-1, -1, -0.001}, {1, 1, 0.001}}; }
+  filament::Box GetBounds() const {
+    return filament::Box().set({-1, -1, -0.001}, {1, 1, 0.001});
+  }
 
  private:
   int num_quads_per_axis_;
@@ -174,7 +176,9 @@ class TriangleBuilder {
     ptr[2] = 2;
   }
 
-  filament::Box GetBounds() const { return {{-1, -1, -0.001}, {1, 1, 0.001}}; }
+  filament::Box GetBounds() const {
+    return filament::Box().set({-1, -1, -0.001}, {1, 1, 0.001});
+  }
 
  private:
   float4 orientation_;
@@ -239,7 +243,9 @@ class LineBoxBuilder {
     ptr[23] = 5;
   }
 
-  filament::Box GetBounds() const { return {{-1, -1, -1}, {1, 1, 1}}; }
+  filament::Box GetBounds() const {
+    return filament::Box().set({-1, -1, -1}, {1, 1, 1});
+  }
 };
 
 class BoxBuilder {
@@ -305,7 +311,9 @@ class BoxBuilder {
     }
   }
 
-  filament::Box GetBounds() const { return {{-1, -1, -1}, {1, 1, 1}}; }
+  filament::Box GetBounds() const {
+    return filament::Box().set({-1, -1, -1}, {1, 1, 1});
+  }
 
  private:
   template <typename F>
@@ -378,7 +386,9 @@ class TubeBuilder {
     }
   }
 
-  filament::Box GetBounds() const { return {{-1, -1, -1}, {1, 1, 1}}; }
+  filament::Box GetBounds() const {
+    return filament::Box().set({-1, -1, -1}, {1, 1, 1});
+  }
 
  private:
   int num_stacks_;
@@ -461,7 +471,9 @@ class ConeBuilder {
     }
   }
 
-  filament::Box GetBounds() const { return {{-1, -1, 0}, {1, 1, 1}}; }
+  filament::Box GetBounds() const {
+    return filament::Box().set({-1, -1, 0}, {1, 1, 1});
+  }
 
  private:
   static VertexType MakeVert(float theta, float radius) {
@@ -520,7 +532,9 @@ class DiskBuilder {
     }
   }
 
-  filament::Box GetBounds() const { return {{-1, -1, -0.001}, {1, 1, 0.001}}; }
+  filament::Box GetBounds() const {
+    return filament::Box().set({-1, -1, -0.001}, {1, 1, 0.001});
+  }
 
  private:
   int num_slices_;
@@ -625,7 +639,9 @@ class SphereBuilder {
     }
   }
 
-  filament::Box GetBounds() const { return {{-1, -1, -1}, {1, 1, 1}}; }
+  filament::Box GetBounds() const {
+    return filament::Box().set({-1, -1, -1}, {1, 1, 1});
+  }
 
  private:
   static VertexType MakeVert(float x, float y, float z) {
@@ -726,7 +742,9 @@ class DomeBuilder {
     }
   }
 
-  filament::Box GetBounds() const { return {{-1, -1, 0}, {1, 1, 1}}; }
+  filament::Box GetBounds() const {
+    return filament::Box().set({-1, -1, 0}, {1, 1, 1});
+  }
 
  private:
   static VertexType MakeVert(float x, float y, float z) {

src/experimental/filament/filament/geom_util.cc

@@ -14,12 +14,16 @@
 
 #include "experimental/filament/filament/geom_util.h"
 
+#include <cfloat>
 #include <cstddef>
 #include <cstdint>
 #include <cstring>
 #include <span>
 
 #include <filament/Engine.h>
+#include <filament/IndexBuffer.h>
+#include <filament/VertexBuffer.h>
+#include <math/vec3.h>
 #include <mujoco/mujoco.h>
 #include "experimental/filament/filament/buffer_util.h"
 #include "experimental/filament/filament/math_util.h"
@@ -27,6 +31,8 @@
 
 namespace mujoco {
 
+using filament::math::float3;
+
 static std::span<const float> GetPositions(const mjModel* model,
                                            const mjvScene* scene,
                                            const mjvGeom& geom) {
@@ -102,11 +108,15 @@ template <typename T>
 static void FillVertices(std::byte* buffer, std::size_t len,
                          std::span<const float> positions,
                          std::span<const float> normals,
-                         std::span<const float> uvs) {
+                         std::span<const float> uvs,
+                         float3* vmin,
+                         float3* vmax) {
   const int num_vertices = len / sizeof(T);
   T* ptr = reinterpret_cast<T*>(buffer);
   for (int i = 0; i < num_vertices; ++i) {
     ptr->position = ReadFloat3(positions.data(), i);
+    *vmin = min(*vmin, ptr->position);
+    *vmax = max(*vmax, ptr->position);
     ptr->orientation = CalculateOrientation(ReadFloat3(normals.data(), i));
     if constexpr (T::kHasUv) {
       ptr->uv.x = uvs[i * 2];
@@ -118,18 +128,21 @@ static void FillVertices(std::byte* buffer, std::size_t len,
 
 static filament::VertexBuffer* BuildVertexBuffer(
     filament::Engine* engine, std::span<const float> positions,
-    std::span<const float> normals, std::span<const float> uvs) {
+    std::span<const float> normals, std::span<const float> uvs, float3* vmin,
+    float3* vmax) {
   const int num_vertices = positions.size() / 3;
   if (uvs.data() != nullptr) {
     using VertexType = VertexWithUv;
     auto fill = [&](std::byte* buffer, std::size_t len) {
-      FillVertices<VertexType>(buffer, len, positions, normals, uvs);
+      FillVertices<VertexType>(buffer, len, positions, normals, uvs, vmin,
+                               vmax);
     };
     return CreateVertexBuffer<VertexType>(engine, num_vertices, fill);
   } else {
     using VertexType = VertexNoUv;
     auto fill = [&](std::byte* buffer, std::size_t len) {
-      FillVertices<VertexType>(buffer, len, positions, normals, uvs);
+      FillVertices<VertexType>(buffer, len, positions, normals, uvs, vmin,
+                               vmax);
     };
     return CreateVertexBuffer<VertexType>(engine, num_vertices, fill);
   }
@@ -163,8 +176,12 @@ FilamentBuffers CreateGeomBuffers(filament::Engine* engine,
   }
 
   FilamentBuffers buffers;
-  buffers.vertex_buffer = BuildVertexBuffer(engine, positions, normals, uvs);
+  float3 vmin = {FLT_MAX, FLT_MAX, FLT_MAX};
+  float3 vmax = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
+  buffers.vertex_buffer =
+      BuildVertexBuffer(engine, positions, normals, uvs, &vmin, &vmax);
   buffers.index_buffer = BuildIndexBuffer(engine, indices, num_indices);
+  buffers.bounds.emplace().set(vmin, vmax);
   return buffers;
 }
 

src/experimental/filament/filament/model_objects.cc

@@ -103,15 +103,16 @@ void ModelObjects::UploadMesh(const mjModel* model, int id) {
   }
 
   FilamentBuffers& buffers = meshes_[id];
-  buffers.vertex_buffer =
+  buffers.vertex_buffer = CreateVertexBuffer(
-      CreateVertexBuffer(engine_, model, id, MeshType::kNormal);
+      engine_, model, id, MeshType::kNormal, &buffers.bounds.emplace());
   buffers.index_buffer =
       CreateIndexBuffer(engine_, model, id, MeshType::kNormal);
 
   if (model->mesh_graphadr[id] >= 0) {
     FilamentBuffers& hull_buffers = convex_hulls_[id];
     hull_buffers.vertex_buffer =
-        CreateVertexBuffer(engine_, model, id, MeshType::kConvexHull);
+        CreateVertexBuffer(engine_, model, id, MeshType::kConvexHull,
+                           &hull_buffers.bounds.emplace());
     hull_buffers.index_buffer =
         CreateIndexBuffer(engine_, model, id, MeshType::kConvexHull);
   }
@@ -160,8 +161,8 @@ void ModelObjects::UploadHeightField(const mjModel* model, int id) {
   }
 
   FilamentBuffers& buffers = height_fields_[id];
-  buffers.vertex_buffer =
+  buffers.vertex_buffer = CreateVertexBuffer(
-      CreateVertexBuffer(engine_, model, id, MeshType::kHeightField);
+      engine_, model, id, MeshType::kHeightField, &buffers.bounds.emplace());
   buffers.index_buffer =
       CreateIndexBuffer(engine_, model, id, MeshType::kHeightField);
 }

src/experimental/filament/filament/model_util.cc

@@ -15,10 +15,12 @@
 #include "experimental/filament/filament/model_util.h"
 
 #include <algorithm>
+#include <cfloat>
 #include <cstddef>
 #include <cstdint>
 #include <limits>
 
+#include <filament/Box.h>
 #include <filament/Engine.h>
 #include <filament/IndexBuffer.h>
 #include <filament/Texture.h>
@@ -48,11 +50,20 @@ static bool UseFaceNormal(const float3& face_normal,
   // clang-format on
 }
 
+static void UpdateBounds(const float3& v, float3* vmin, float3* vmax) {
+  vmin->x = std::min(vmin->x, v.x);
+  vmin->y = std::min(vmin->y, v.y);
+  vmin->z = std::min(vmin->z, v.z);
+  vmax->x = std::max(vmax->x, v.x);
+  vmax->y = std::max(vmax->y, v.y);
+  vmax->z = std::max(vmax->z, v.z);
+}
+
 template <typename T>
 static void FillConvexHullBuffer(T* ptr, std::size_t num, const mjModel* model,
-                                int meshid) {
+                                 int meshid, float3* vmin, float3* vmax) {
   const int numvert = model->mesh_graph[model->mesh_graphadr[meshid]];
-  const int numface = model->mesh_graph[model->mesh_graphadr[meshid]+1];
+  const int numface = model->mesh_graph[model->mesh_graphadr[meshid] + 1];
 
   const int vertadr = model->mesh_vertadr[meshid];
   const float* vertices = model->mesh_vert + (3 * vertadr);
@@ -65,13 +76,18 @@ static void FillConvexHullBuffer(T* ptr, std::size_t num, const mjModel* model,
   }
 
   for (int face = 0; face < numface; ++face) {
-    int j = model->mesh_graphadr[meshid] + 2 + 3*numvert + 3*numface + 3*face;
+    int j =
+        model->mesh_graphadr[meshid] + 2 + 3 * numvert + 3 * numface + 3 * face;
 
     const float3 p1 = ReadFloat3(vertices, model->mesh_graph[j + 0]);
     const float3 p2 = ReadFloat3(vertices, model->mesh_graph[j + 1]);
     const float3 p3 = ReadFloat3(vertices, model->mesh_graph[j + 2]);
     const float4 orientation = CalculateOrientation(p1, p2, p3);
 
+    UpdateBounds(p1, vmin, vmax);
+    UpdateBounds(p2, vmin, vmax);
+    UpdateBounds(p3, vmin, vmax);
+
     ptr->position = p1;
     ptr->orientation = orientation;
     if constexpr (T::kHasUv) {
@@ -97,7 +113,7 @@ static void FillConvexHullBuffer(T* ptr, std::size_t num, const mjModel* model,
 
 template <typename T>
 static void FillMeshBuffer(T* ptr, std::size_t num, const mjModel* model,
-                          int meshid) {
+                           int meshid, float3* vmin, float3* vmax) {
   const int faceadr = model->mesh_faceadr[meshid];
   const int facenum = model->mesh_facenum[meshid];
   if (num != facenum * 3) {
@@ -118,8 +134,12 @@ static void FillMeshBuffer(T* ptr, std::size_t num, const mjModel* model,
     const float3 p1 = ReadFloat3(vertices, model->mesh_face[face + 0]);
     const float3 p2 = ReadFloat3(vertices, model->mesh_face[face + 1]);
     const float3 p3 = ReadFloat3(vertices, model->mesh_face[face + 2]);
-    const float3 face_normal = CalculateNormal(p1, p2, p3);
 
+    UpdateBounds(p1, vmin, vmax);
+    UpdateBounds(p2, vmin, vmax);
+    UpdateBounds(p3, vmin, vmax);
+
+    const float3 face_normal = CalculateNormal(p1, p2, p3);
     const float3 n1 = ReadFloat3(normals, model->mesh_facenormal[face + 0]);
     const float3 n2 = ReadFloat3(normals, model->mesh_facenormal[face + 1]);
     const float3 n3 = ReadFloat3(normals, model->mesh_facenormal[face + 2]);
@@ -160,7 +180,8 @@ static void FillMeshBuffer(T* ptr, std::size_t num, const mjModel* model,
 }
 
 static void FillHeightFieldBuffer(VertexNoUv* ptr, std::size_t num,
-                                  const mjModel* model, int hfieldid) {
+                                  const mjModel* model, int hfieldid,
+                                  float3* vmin, float3* vmax) {
   int count = 0;
   auto append_tri = [&](float3 a, float3 b, float3 c) {
     float4 orientation = CalculateOrientation(a, b, c);
@@ -173,6 +194,10 @@ static void FillHeightFieldBuffer(VertexNoUv* ptr, std::size_t num,
     ptr[count].position = c;
     ptr[count].orientation = orientation;
     ++count;
+
+    UpdateBounds(a, vmin, vmax);
+    UpdateBounds(b, vmin, vmax);
+    UpdateBounds(c, vmin, vmax);
   };
   auto append_quad = [&](float3 a, float3 b, float3 c, float3 d) {
     append_tri(a, b, d);
@@ -186,7 +211,7 @@ static void FillHeightFieldBuffer(VertexNoUv* ptr, std::size_t num,
   const float width = 0.5f * (ncol - 1);
   float sz[4];
   for (int i = 0; i < 4; ++i) {
-    sz[i] = static_cast<float>(model->hfield_size[4 * hfieldid  + i]);
+    sz[i] = static_cast<float>(model->hfield_size[4 * hfieldid + i]);
   }
 
   auto get_pos = [=](int r, int c) {
@@ -254,8 +279,8 @@ static void FillHeightFieldBuffer(VertexNoUv* ptr, std::size_t num,
   }
   // Build the right edge.
   for (int row = 0; row < nrow - 1; ++row) {
-    const float3 a = get_pos(row + 1, ncol-1);
+    const float3 a = get_pos(row + 1, ncol - 1);
-    const float3 b = get_pos(row, ncol-1);
+    const float3 b = get_pos(row, ncol - 1);
     const float3 c = {b.x, b.y, -sz[3]};
     const float3 d = {a.x, a.y, -sz[3]};
     append_quad(a, b, c, d);
@@ -270,8 +295,8 @@ static void FillHeightFieldBuffer(VertexNoUv* ptr, std::size_t num,
   }
   // Build the back edge.
   for (int col = 0; col < ncol - 1; ++col) {
-    const float3 a = get_pos(nrow-1, col + 1);
+    const float3 a = get_pos(nrow - 1, col + 1);
-    const float3 b = get_pos(nrow-1, col);
+    const float3 b = get_pos(nrow - 1, col);
     const float3 c = {b.x, b.y, -sz[3]};
     const float3 d = {a.x, a.y, -sz[3]};
     append_quad(a, b, c, d);
@@ -286,9 +311,7 @@ static void FillHeightFieldBuffer(VertexNoUv* ptr, std::size_t num,
       const float x1 = sz[0] * ((col + 1) / base_width - 1.0f);
       const float y0 = sz[1] * ((row + 0) / base_height - 1.0f);
       const float y1 = sz[1] * ((row + 1) / base_height - 1.0f);
-      append_quad({x0, y0, -sz[3]},
+      append_quad({x0, y0, -sz[3]}, {x0, y1, -sz[3]}, {x1, y1, -sz[3]},
-                  {x0, y1, -sz[3]},
-                  {x1, y1, -sz[3]},
                   {x1, y0, -sz[3]});
     }
   }
@@ -305,7 +328,7 @@ static int CalculateHeightFieldVertexCount(const mjModel* model, int hfieldid) {
   // we need. But, in general...
 
   // We use 4 triangles (i.e. 12 vertices) per quad.
-  const int surface_count = 12 * (nrow-1) * (ncol-1);
+  const int surface_count = 12 * (nrow - 1) * (ncol - 1);
   // We use 1 quad (i.e. 6 vertices) per edge element. We double this because
   // we have two edges per dimension (e.g. left/right and front/back).
   const int edge_count = (12 * (nrow - 1)) + (12 * (ncol - 1));
@@ -322,17 +345,23 @@ template <typename T, typename FillFn>
 static filament::VertexBuffer* CreateVertexBuffer(filament::Engine* engine,
                                                   const mjModel* model, int id,
                                                   int vertex_count,
-                                                  FillFn fill_fn) {
+                                                  FillFn fill_fn,
-  return CreateVertexBuffer<T>(
+                                                  filament::Box* bounds) {
+  float3 vmin = {FLT_MAX, FLT_MAX, FLT_MAX};
+  float3 vmax = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
+  filament::VertexBuffer* buffer = CreateVertexBuffer<T>(
       engine, vertex_count, [&](std::byte* buffer, std::size_t num_bytes) {
         auto* ptr = reinterpret_cast<T*>(buffer);
-        fill_fn(ptr, num_bytes / sizeof(T), model, id);
+        fill_fn(ptr, num_bytes / sizeof(T), model, id, &vmin, &vmax);
       });
+  bounds->set(vmin, vmax);
+  return buffer;
 }
 
 filament::VertexBuffer* CreateVertexBuffer(filament::Engine* engine,
                                            const mjModel* model, int id,
-                                           MeshType mesh_type) {
+                                           MeshType mesh_type,
+                                           filament::Box* bounds) {
   if (id < 0) {
     mju_error("Invalid mesh index %d", id);
     return nullptr;
@@ -376,11 +405,13 @@ filament::VertexBuffer* CreateVertexBuffer(filament::Engine* engine,
     switch (mesh_type) {
       case MeshType::kNormal:
         return CreateVertexBuffer<VertexType>(engine, model, id, vertex_count,
-                                              FillMeshBuffer<VertexType>);
+                                              FillMeshBuffer<VertexType>,
+                                              bounds);
         break;
       case MeshType::kConvexHull:
         return CreateVertexBuffer<VertexType>(engine, model, id, vertex_count,
-                                              FillConvexHullBuffer<VertexType>);
+                                              FillConvexHullBuffer<VertexType>,
+                                              bounds);
         break;
       case MeshType::kHeightField:
         mju_error("Height fields do not support UV coordinates.");
@@ -391,15 +422,17 @@ filament::VertexBuffer* CreateVertexBuffer(filament::Engine* engine,
     switch (mesh_type) {
       case MeshType::kNormal:
         return CreateVertexBuffer<VertexType>(engine, model, id, vertex_count,
-                                              FillMeshBuffer<VertexType>);
+                                              FillMeshBuffer<VertexType>,
+                                              bounds);
         break;
       case MeshType::kConvexHull:
         return CreateVertexBuffer<VertexType>(engine, model, id, vertex_count,
-                                              FillConvexHullBuffer<VertexType>);
+                                              FillConvexHullBuffer<VertexType>,
+                                              bounds);
         break;
       case MeshType::kHeightField:
         return CreateVertexBuffer<VertexType>(engine, model, id, vertex_count,
-                                              FillHeightFieldBuffer);
+                                              FillHeightFieldBuffer, bounds);
         break;
     }
   }

src/experimental/filament/filament/model_util.h

@@ -17,6 +17,7 @@
 
 #include <string_view>
 
+#include <filament/Box.h>
 #include <filament/Engine.h>
 #include <filament/IndexBuffer.h>
 #include <filament/Texture.h>
@@ -45,7 +46,8 @@ enum class TextureType {
 // Generates a filament VertexBuffer for a given mesh in the mjModel.
 filament::VertexBuffer* CreateVertexBuffer(filament::Engine* engine,
                                            const mjModel* model, int id,
-                                           MeshType mesh_type);
+                                           MeshType mesh_type,
+                                           filament::Box* bounds);
 
 // Generates a filament IndexBuffer for a given mesh in the mjModel.
 filament::IndexBuffer* CreateIndexBuffer(filament::Engine* engine,

src/experimental/filament/filament/renderables.cc

@@ -105,13 +105,16 @@ utils::Entity Renderables::CreateEntity(const FilamentBuffers& buffers) {
   if (material_instance_) {
     builder.material(0, material_instance_);
   }
-  builder.boundingBox(buffers.bounds)
+  if (buffers.bounds.has_value()) {
-      .culling(false)
+    builder.boundingBox(buffers.bounds.value());
-      .castShadows(cast_shadows_)
+  } else {
-      .receiveShadows(receive_shadows_)
+    builder.culling(false);
-      .layerMask(0xff, layer_mask_)
+  }
-      .priority(priority_)
+  builder.castShadows(cast_shadows_);
-      .screenSpaceContactShadows(true);
+  builder.receiveShadows(receive_shadows_);
+  builder.layerMask(0xff, layer_mask_);
+  builder.priority(priority_);
+  builder.screenSpaceContactShadows(true);;
 
   builder.build(*engine_, entity);
   if (assigned_scene_) {