diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index 100d0fee..4ffc4ed6 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -102,6 +102,8 @@ mat4 CalculateClipFromWorld(const mjrRect& viewport, const mjrCamera& cam) { } void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { + const mjModel* model = model_objects_->GetModel(); + mjtNum hpos[3], hfwd[3]; float headpos[3], gazedir[3]; mjv_cameraInModel(hpos, hfwd, nullptr, scene); @@ -125,6 +127,84 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { } } + // Draw flex edges and vertices as separate renderables. + if (geom->type == mjGEOM_FLEX && + (!scene->flexskinopt || model->flex_dim[geom->objid] == 1)) { + mjrfMaterial material; + mjrf_defaultMaterial(&material); + material.color[0] = model->flex_rgba[4 * geom->objid + 0]; + material.color[1] = model->flex_rgba[4 * geom->objid + 1]; + material.color[2] = model->flex_rgba[4 * geom->objid + 2]; + material.color[3] = model->flex_rgba[4 * geom->objid + 3]; + + mjrfRenderableParams params; + mjrf_defaultRenderableParams(¶ms); + + const int vertadr = scene->flexvertadr[geom->objid]; + const int vertnum = scene->flexvertnum[geom->objid]; + const float radius = model->flex_radius[geom->objid]; + + if (scene->flexvertopt) { + // Use small spheres to represent vertices. + const float rot[] = {1.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 1.f}; + const float size[] = {radius, radius, radius}; + for (int v = vertadr; v < vertadr + vertnum; ++v) { + auto vertex = CreateRenderable(ctx_, params); + mjrf_setRenderableGeomMesh(vertex.get(), mjGEOM_SPHERE, 3, 3, 3); + mjrf_setRenderableMaterial(vertex.get(), &material); + mjrf_setRenderableSize(vertex.get(), size); + mjrf_setRenderableTransform(vertex.get(), scene->flexvert + 3*v, rot); + mjrf_addRenderableToScene(scene_.get(), vertex.get()); + renderables_.push_back(std::move(vertex)); + } + } + + if (scene->flexedgeopt) { + const int edgeadr = scene->flexedgeadr[geom->objid]; + const int edgenum = scene->flexedgenum[geom->objid]; + + // Use small thin cylinders to represent the edges. + for (int e = edgeadr; e < edgeadr + edgenum; ++e) { + const float* v1 = scene->flexvert + 3 * (vertadr + scene->flexedge[2*e]); + const float* v2 = scene->flexvert + 3 * (vertadr + scene->flexedge[2*e+1]); + const mjtNum vec[3] = {v2[0] - v1[0], v2[1] - v1[1], v2[2] - v1[2]}; + + const float pos[3]{ + (v1[0] + v2[0]) * 0.5f, + (v1[1] + v2[1]) * 0.5f, + (v1[2] + v2[2]) * 0.5f, + }; + + mjtNum quat[4]; + mju_quatZ2Vec(quat, vec); + mjtNum edgemat[9]; + mju_quat2Mat(edgemat, quat); + const float rot[9] = { + static_cast(edgemat[0]), + static_cast(edgemat[1]), + static_cast(edgemat[2]), + static_cast(edgemat[3]), + static_cast(edgemat[4]), + static_cast(edgemat[5]), + static_cast(edgemat[6]), + static_cast(edgemat[7]), + static_cast(edgemat[8]), + }; + + const float len = static_cast(mju_norm3(vec)); + const float size[3] = {radius, radius, len * 0.5f}; + + auto vertex = CreateRenderable(ctx_, params); + mjrf_setRenderableGeomMesh(vertex.get(), mjGEOM_CYLINDER, 3, 3, 3); + mjrf_setRenderableMaterial(vertex.get(), &material); + mjrf_setRenderableSize(vertex.get(), size); + mjrf_setRenderableTransform(vertex.get(), pos, rot); + mjrf_addRenderableToScene(scene_.get(), vertex.get()); + renderables_.push_back(std::move(vertex)); + } + } + } + if (geom->type == mjGEOM_FLEX || geom->type == mjGEOM_SKIN) { if (!scene_objects_->CreateSkinFlexMesh(scene, model_objects_->GetModel(), *geom)) {