From 30a99de3b9b326585b0073844f155ac7011f1f2b Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 5 Nov 2025 07:32:31 -0800 Subject: [PATCH] Remove dependencies on mjvScene for interactions. Interactions (like moving the camera or mouse picking) no longer depend on needing an mjvScene. Instead, everything is done relative to the camera directly. This duplicates some code in engine_vis_visualize which we will try to resolve and recombine at a future time. PiperOrigin-RevId: 828461259 Change-Id: If26329c1a7c37516f0213b18056c3e31760f60ae --- src/experimental/studio/app.cc | 69 ++- src/experimental/toolbox/interaction.cc | 570 +++++++++++++++++++++++- src/experimental/toolbox/interaction.h | 18 +- 3 files changed, 612 insertions(+), 45 deletions(-) diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index 5b3ff75a..d92aa038 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -58,16 +58,15 @@ namespace mujoco::studio { static constexpr toolbox::Window::Config kWindowConfig = { #if defined(USE_FILAMENT_VULKAN) - .render_config = toolbox::Window::RenderConfig::kFilamentVulkan, + .render_config = toolbox::Window::RenderConfig::kFilamentVulkan, #elif defined(USE_FILAMENT_OPENGL) - .render_config = toolbox::Window::RenderConfig::kFilamentOpenGL, + .render_config = toolbox::Window::RenderConfig::kFilamentOpenGL, #elif defined(USE_CLASSIC_OPENGL) - .render_config = toolbox::Window::RenderConfig::kClassicOpenGL, + .render_config = toolbox::Window::RenderConfig::kClassicOpenGL, #endif - .enable_keyboard = true, + .enable_keyboard = true, }; - static void ToggleFlag(mjtByte& flag) { flag = flag ? 0 : 1; } static void ToggleWindow(bool& window) { @@ -233,10 +232,6 @@ void App::HandleMouseEvents() { return; } - mjModel* model = physics_->GetModel(); - mjData* data = physics_->GetData(); - mjvScene& scene = renderer_->GetScene(); - // Normalize mouse positions and movement to display size. const float mouse_x = io.MousePos.x / io.DisplaySize.x; const float mouse_y = io.MousePos.y / io.DisplaySize.y; @@ -258,35 +253,36 @@ void App::HandleMouseEvents() { } // Mouse scroll. - if (model && mouse_scroll != 0.0f) { - mjv_moveCamera(model, mjMOUSE_ZOOM, 0, mouse_scroll, &scene, &camera_); + if (Model() && mouse_scroll != 0.0f) { + toolbox::MoveCamera(Model(), Data(), &camera_, mjMOUSE_ZOOM, 0, + mouse_scroll); } // Mouse drag. - if (model && data && action != mjMOUSE_NONE && + if (Model() && Data() && action != mjMOUSE_NONE && (mouse_dx != 0.0f || mouse_dy != 0.0f)) { // If ctrl is pressed, move the perturbation, otherwise move the camera_. if (io.KeyCtrl) { if (perturb_.select > 0) { - const int active = + const mjtPertBit active = action == mjMOUSE_MOVE_V ? mjPERT_TRANSLATE : mjPERT_ROTATE; if (active != perturb_.active) { - mjv_initPerturb(model, data, &scene, &perturb_); - perturb_.active = active; + toolbox::InitPerturb(Model(), Data(), &camera_, &perturb_, active); } - mjv_movePerturb(model, data, action, mouse_dx, mouse_dy, &scene, - &perturb_); + toolbox::MovePerturb(Model(), Data(), &camera_, &perturb_, action, + mouse_dx, mouse_dy); } } else { - mjv_moveCamera(model, action, mouse_dx, mouse_dy, &scene, &camera_); + toolbox::MoveCamera(Model(), Data(), &camera_, action, mouse_dx, + mouse_dy); } } // Left double click. - if (data && ImGui::IsMouseDoubleClicked(ImGuiMouseButton_Left)) { - toolbox::PickResult picked = toolbox::Pick( - model, data, &camera_, mouse_x, mouse_y, window_->GetAspectRatio(), - &renderer_->GetScene(), &vis_options_); + if (Data() && ImGui::IsMouseDoubleClicked(ImGuiMouseButton_Left)) { + toolbox::PickResult picked = + toolbox::Pick(Model(), Data(), &camera_, mouse_x, mouse_y, + window_->GetAspectRatio(), &vis_options_); if (picked.body >= 0) { perturb_.select = picked.body; perturb_.flexselect = picked.flex; @@ -294,8 +290,9 @@ void App::HandleMouseEvents() { // Compute the local position of the selected object in the world. mjtNum tmp[3]; - mju_sub3(tmp, picked.point, data->xpos + 3 * picked.body); - mju_mulMatTVec(perturb_.localpos, data->xmat + 9 * picked.body, tmp, 3, 3); + mju_sub3(tmp, picked.point, Data()->xpos + 3 * picked.body); + mju_mulMatTVec(perturb_.localpos, Data()->xmat + 9 * picked.body, tmp, 3, + 3); } else { perturb_.select = 0; perturb_.flexselect = -1; @@ -305,9 +302,9 @@ void App::HandleMouseEvents() { // Right double click. if (ImGui::IsMouseDoubleClicked(ImGuiMouseButton_Right)) { - toolbox::PickResult picked = toolbox::Pick( - model, data, &camera_, mouse_x, mouse_y, window_->GetAspectRatio(), - &renderer_->GetScene(), &vis_options_); + toolbox::PickResult picked = + toolbox::Pick(Model(), Data(), &camera_, mouse_x, mouse_y, + window_->GetAspectRatio(), &vis_options_); mju_copy3(camera_.lookat, picked.point); if (picked.body > 0 && io.KeyCtrl) { camera_.type = mjCAMERA_TRACKING; @@ -422,28 +419,28 @@ void App::HandleKeyboardEvents() { // Camera wasd controls. if (!ui_.classic_ui) { - mjvScene& scene = renderer_->GetScene(); - mjModel* model = Model(); + const mjModel* model = Model(); + const mjData* data = Data(); // Move (dolly) forward/backward using W and S keys. if (ImGui::IsKeyDown(ImGuiKey_W)) { - mjv_moveCamera(model, mjMOUSE_MOVE_H_REL, 0, 0.01, &scene, &camera_); + toolbox::MoveCamera(model, data, &camera_, mjMOUSE_MOVE_H_REL, 0, 0.01); } else if (ImGui::IsKeyDown(ImGuiKey_S)) { - mjv_moveCamera(model, mjMOUSE_MOVE_H_REL, 0, -0.01, &scene, &camera_); + toolbox::MoveCamera(model, data, &camera_, mjMOUSE_MOVE_H_REL, 0, -0.01); } // Strafe (truck) left/right using A dna D keys. if (ImGui::IsKeyDown(ImGuiKey_A)) { - mjv_moveCamera(model, mjMOUSE_MOVE_H_REL, -0.01, 0, &scene, &camera_); + toolbox::MoveCamera(model, data, &camera_, mjMOUSE_MOVE_H_REL, -0.01, 0); } else if (ImGui::IsKeyDown(ImGuiKey_D)) { - mjv_moveCamera(model, mjMOUSE_MOVE_H_REL, 0.01, 0, &scene, &camera_); + toolbox::MoveCamera(model, data, &camera_, mjMOUSE_MOVE_H_REL, 0.01, 0); } // Move (pedestal) up/down using Q and E keys. if (ImGui::IsKeyDown(ImGuiKey_Q)) { - mjv_moveCamera(model, mjMOUSE_MOVE_V_REL, 0, 0.01, &scene, &camera_); + toolbox::MoveCamera(model, data, &camera_, mjMOUSE_MOVE_V_REL, 0, 0.01); } else if (ImGui::IsKeyDown(ImGuiKey_E)) { - mjv_moveCamera(model, mjMOUSE_MOVE_V_REL, 0, -0.01, &scene, &camera_); + toolbox::MoveCamera(model, data, &camera_, mjMOUSE_MOVE_V_REL, 0, -0.01); } } @@ -459,7 +456,7 @@ void App::HandleKeyboardEvents() { if (ImGui_IsChordJustPressed(ImGuiKey_Escape)) { ui_.camera_idx = toolbox::SetCamera(model, &camera_, 0); } else if (ImGui_IsChordJustPressed(ImGuiKey_LeftBracket)) { - ui_.camera_idx = toolbox::SetCamera(model, &camera_,ui_.camera_idx - 1); + ui_.camera_idx = toolbox::SetCamera(model, &camera_, ui_.camera_idx - 1); } else if (ImGui_IsChordJustPressed(ImGuiKey_RightBracket)) { ui_.camera_idx = toolbox::SetCamera(model, &camera_, ui_.camera_idx + 1); } diff --git a/src/experimental/toolbox/interaction.cc b/src/experimental/toolbox/interaction.cc index b1425935..3abe2ded 100644 --- a/src/experimental/toolbox/interaction.cc +++ b/src/experimental/toolbox/interaction.cc @@ -15,21 +15,579 @@ #include "experimental/toolbox/interaction.h" #include +#include +#include #include +#include "engine/engine_util_errmem.h" +#include "engine/engine_util_misc.h" +#include "engine/engine_vis_visualize.h" namespace mujoco::toolbox { -PickResult Pick(const mjModel* m, const mjData* d, const mjvCamera* camera, - float x, float y, float aspect_ratio, const mjvScene* scene, - const mjvOption* vis_options) { +static mjtNum CalculateMovementScale(const mjModel* m, const mjvCamera* cam) { + float zclip[2] = {0, 0}, zver[2] = {0, 0}; + mjv_cameraFrustum(zver, nullptr, zclip, m, cam); + if (cam->orthographic) { + // TODO(b/346130949): multiply by mystery coefficient + return (zver[1] + zver[0]) * 0.15; + } else if (zclip[0] >= mjMINVAL) { + return (zver[1] + zver[0]) / zclip[0]; + } else { + mjERROR("mjvScene frustum_near too small"); + return 0; + } +} + +static void AlignToCamera(mjtNum res[3], mjtMouse action, mjtNum dx, mjtNum dy, + const mjtNum forward[3]) { + mjtNum vec[3]; + switch (action) { + case mjMOUSE_ROTATE_V: + vec[0] = dy; + vec[1] = 0; + vec[2] = dx; + break; + case mjMOUSE_ROTATE_H: + vec[0] = dy; + vec[1] = dx; + vec[2] = 0; + break; + case mjMOUSE_MOVE_V: + case mjMOUSE_MOVE_V_REL: + vec[0] = dx; + vec[1] = 0; + vec[2] = -dy; + break; + case mjMOUSE_MOVE_H: + case mjMOUSE_MOVE_H_REL: + vec[0] = dx; + vec[1] = -dy; + vec[2] = 0; + break; + case mjMOUSE_ZOOM: + break; + default: + mjERROR("unexpected mouse action %d in AlignToCamera", action); + } + + // call 3D converter + mjv_alignToCamera(res, vec, forward); +} + +void InitPerturb(const mjModel* m, const mjData* d, const mjvCamera* cam, + mjvPerturb* pert, mjtPertBit active) { + // compute selection point in world coordinates + const int sel = pert->select; + mjtNum selpos[3]; + mju_mulMatVec3(selpos, d->xmat + 9 * sel, pert->localpos); + mju_addTo3(selpos, d->xpos + 3 * sel); + + // compute average spatial inertia at selection point + const int nv = m->nv; + std::vector sqrtInvD(nv); + for (int i = 0; i < nv; i++) { + sqrtInvD[i] = mju_sqrt(d->qLDiagInv[i]); + } + + std::vector jac(3 * nv); + mj_jac(m, d, jac.data(), nullptr, selpos, sel); + + std::vector jacM2(3 * nv); + mj_solveM2(m, const_cast(d), jacM2.data(), jac.data(), + sqrtInvD.data(), 3); + mjtNum invmass = mju_dot(jacM2.data() + 0 * nv, jacM2.data() + 0 * nv, nv) + + mju_dot(jacM2.data() + 1 * nv, jacM2.data() + 1 * nv, nv) + + mju_dot(jacM2.data() + 2 * nv, jacM2.data() + 2 * nv, nv); + pert->localmass = (invmass == 0) ? 1 : 3 / mju_max(invmass, mjMINVAL); + + // scale localmass with flex average number of edges per vertex + if (pert->flexselect >= 0 && !m->flex_rigid[pert->flexselect]) { + pert->localmass *= (2.0 * m->flex_edgenum[pert->flexselect]) / + (mjtNum)m->flex_vertnum[pert->flexselect]; + } + + // copy + mju_copy3(pert->refpos, d->xipos + 3 * sel); + mju_mulQuat(pert->refquat, d->xquat + 4 * sel, m->body_iquat + 4 * sel); + mju_copy3(pert->refselpos, selpos); + + // get camera info + mjtNum headpos[3], forward[3]; + mjv_cameraFrame(headpos, forward, nullptr, nullptr, d, cam); + + // compute scaling: rendered pert->refselpos displacement = mouse displacement + mjtNum dif[3]; + mju_sub3(dif, pert->refselpos, headpos); + pert->scale = CalculateMovementScale(m, cam) * mju_dot3(dif, forward); + pert->active = active; +} + +void MovePerturb(const mjModel* m, const mjData* d, const mjvCamera* cam, + mjvPerturb* pert, mjtMouse action, mjtNum reldx, + mjtNum reldy) { + const mjtNum xaxis[3] = {1, 0, 0}; + const mjtNum yaxis[3] = {0, 1, 0}; + const mjtNum zaxis[3] = {0, 0, 1}; + + int sel = pert->select; + const mjtNum* xmat = d->xmat + 9 * sel; + mjtNum forward[3], vec[3], scl, q1[4], xiquat[4]; + + // get camera info and align + mjv_cameraFrame(nullptr, forward, nullptr, nullptr, d, cam); + AlignToCamera(vec, action, reldx, reldy, forward); + + // process action + switch ((mjtMouse)action) { + case mjMOUSE_MOVE_V: + case mjMOUSE_MOVE_H: + // move along world-space horizontal/vertical planes relative to camera + mju_addToScl3(pert->refpos, vec, pert->scale); + mju_addToScl3(pert->refselpos, vec, pert->scale); + break; + + case mjMOUSE_MOVE_V_REL: + case mjMOUSE_MOVE_H_REL: + // move along object's local coordinate frame + if (action == mjMOUSE_MOVE_H_REL) { + mju_mulMatVec3(vec, xmat, xaxis); + mju_addToScl3(pert->refpos, vec, pert->scale * reldy); + mju_addToScl3(pert->refselpos, vec, pert->scale * reldy); + } else { + mju_mulMatVec3(vec, xmat, zaxis); + mju_addToScl3(pert->refpos, vec, pert->scale * reldy); + mju_addToScl3(pert->refselpos, vec, pert->scale * reldy); + } + + mju_mulMatVec3(vec, xmat, yaxis); + mju_addToScl3(pert->refpos, vec, pert->scale * reldx); + mju_addToScl3(pert->refselpos, vec, pert->scale * reldx); + break; + + case mjMOUSE_ROTATE_V: + case mjMOUSE_ROTATE_H: + // normalize vector, get length + scl = mju_normalize3(vec); + + // make quaternion and apply + mju_axisAngle2Quat(q1, vec, scl * mjPI * 2); + mju_mulQuat(pert->refquat, q1, pert->refquat); + mju_normalize4(pert->refquat); + + // compute xiquat + mju_mulQuat(xiquat, d->xquat + 4 * sel, m->body_iquat + 4 * sel); + + // limit rotation relative to selected body + if (sel > 0 && sel < m->nbody) { + // q2 = neg(selbody) * refquat + mjtNum q2[4]; + mju_negQuat(q1, xiquat); + mju_mulQuat(q2, q1, pert->refquat); + + // convert q2 to axis-angle + mjtNum dif[3]; + mju_quat2Vel(dif, q2, 1); + scl = mju_normalize3(dif); + + // check limit: +/- 90 deg allowed + if (scl < -mjPI * 0.5 || scl > mjPI * 0.5) { + // clamp angle + scl = mju_max(-mjPI * 0.5, mju_min(mjPI * 0.5, scl)); + + // reconstruct q2 + mju_axisAngle2Quat(q2, dif, scl); + + // set refquat = selbody * q2_new + mju_mulQuat(pert->refquat, xiquat, q2); + } + } + break; + + case mjMOUSE_ZOOM: + break; + + default: + mjERROR("unexpected mouse action %d", action); + } +} + +void MoveCamera(const mjModel* m, const mjData* d, mjvCamera* cam, + mjtMouse action, mjtNum reldx, mjtNum reldy) { + if (cam->type == mjCAMERA_FIXED) { + return; + } + + mjtNum headpos[3], forward[3], up[3], right[3]; + mjtNum vec[3], dif[3], scl; + + switch (action) { + case mjMOUSE_ROTATE_V: + case mjMOUSE_ROTATE_H: + cam->azimuth -= reldx * 180.0; + cam->elevation -= reldy * 180.0; + break; + + case mjMOUSE_MOVE_V: + case mjMOUSE_MOVE_H: + // do not move lookat point of tracking camera + if (cam->type == mjCAMERA_TRACKING) { + return; + } + + // get camera info and align + mjv_cameraFrame(headpos, forward, nullptr, nullptr, d, cam); + AlignToCamera(vec, action, reldx, reldy, forward); + + // compute scaling: rendered lookat displacement = mouse displacement + mju_sub3(dif, cam->lookat, headpos); + scl = CalculateMovementScale(m, cam) * mju_dot3(dif, forward); + + // move lookat point in opposite direction + mju_addToScl3(cam->lookat, vec, -scl); + break; + + case mjMOUSE_ZOOM: + cam->distance -= mju_log(1 + cam->distance / m->stat.extent / 3) * reldy * + 9 * m->stat.extent; + break; + + case mjMOUSE_MOVE_V_REL: + case mjMOUSE_MOVE_H_REL: + // do not move lookat point of tracking camera + if (cam->type == mjCAMERA_TRACKING) { + return; + } + + mjv_cameraFrame(headpos, forward, up, nullptr, d, cam); + mju_cross(right, forward, up); + + // y-axis movement moves forward/backward (ie. camera dolly) on horizontal + // plane or up/down (ie. camera pedestal) on vertical plane + mju_addToScl3(cam->lookat, (action == mjMOUSE_MOVE_V_REL) ? up : forward, + reldy); + + // x-axis movement strafes left/right (ie. camera truck) + mju_addToScl3(cam->lookat, right, reldx); + + break; + + default: + mjERROR("unexpected action %d", action); + } + + // clamp camera parameters + if (cam->azimuth > 180) { + cam->azimuth -= 360; + } + if (cam->azimuth < -180) { + cam->azimuth += 360; + } + if (cam->elevation > 89) { + cam->elevation = 89; + } + if (cam->elevation < -89) { + cam->elevation = -89; + } + if (cam->distance < 0.01 * m->stat.extent) { + cam->distance = 0.01 * m->stat.extent; + } + if (cam->distance > 100 * m->stat.extent) { + cam->distance = 100 * m->stat.extent; + } +} + +static void MakePickRay(mjtNum pos[3], mjtNum ray[3], const mjModel* m, + const mjData* d, const mjvCamera* camera, float relx, + float rely, float aspect_ratio) { + mjtNum forward[3], up[3], right[3]; + mjv_cameraFrame(pos, forward, up, right, d, camera); + + float zver[2], zhor[2], zclip[2] = {0, 0}; + mjv_cameraFrustum(zver, zhor, zclip, m, camera); + + // compute frustum halfwidth to match viewport aspect ratio + mjtNum half_width = 0.5 * aspect_ratio * (zver[0] + zver[1]); + mjtNum frustum_center = (zhor[1] - zhor[0]) / 2; + + // compute up and right offsets from normalized cursor + mjtNum d_up = -zver[0] + rely * (zver[0] + zver[1]); + mjtNum d_right = frustum_center + (2 * relx - 1) * half_width; + + if (camera->orthographic) { + mju_copy3(ray, forward); + mju_addToScl3(pos, up, d_up); + mju_addToScl3(pos, right, d_right); + } else { + mju_scl3(ray, forward, zclip[0]); + mju_addToScl3(ray, up, d_up); + mju_addToScl3(ray, right, d_right); + mju_normalize3(ray); + } +} + +static PickResult PickGeom(const mjModel* m, const mjData* d, + const mjtNum ray_pos[3], const mjtNum ray_dir[3], + const mjvOption* vis_options) { PickResult result; - result.body = - mjv_select(m, d, vis_options, aspect_ratio, x, 1.0f - y, scene, - result.point, &result.geom, &result.flex, &result.skin); + result.dist = mj_ray(m, d, ray_pos, ray_dir, vis_options->geomgroup, + vis_options->flags[mjVIS_STATIC], -1, &result.geom); + mju_addScl3(result.point, ray_pos, ray_dir, result.dist); + result.body = m->geom_bodyid[result.geom]; return result; } +static PickResult PickFlex(const mjModel* m, const mjData* d, + const mjtNum ray_pos[3], const mjtNum ray_dir[3], + const mjvOption* vis_options) { + const mjtByte flag_vert = vis_options->flags[mjVIS_FLEXVERT]; + const mjtByte flag_edge = vis_options->flags[mjVIS_FLEXEDGE]; + const mjtByte flag_face = vis_options->flags[mjVIS_FLEXFACE]; + const mjtByte flag_skin = vis_options->flags[mjVIS_FLEXSKIN]; + + PickResult result; + if (!flag_vert && !flag_edge && !flag_face && !flag_skin) { + return result; + } + + for (int i = 0; i < m->nflex; i++) { + int vertid; + const mjtNum test_dist = + mju_rayFlex(m, d, vis_options->flex_layer, flag_vert, flag_edge, + flag_face, flag_skin, i, ray_pos, ray_dir, &vertid); + + if (test_dist < 0) { + continue; + } else if (result.dist >= 0 && test_dist >= result.dist) { + continue; + } + + result.dist = test_dist; + if (m->flex_interp[i]) { + const mjtNum* coord = m->flex_vert0 + 3 * (m->flex_vertadr[i] + vertid); + mjtNum w = 0; + int nodeid = -1; + int nstart = m->flex_nodeadr[i]; + int nend = nstart + m->flex_nodenum[i]; + for (int j = nstart; j < nend; j++) { + if (mju_evalBasis(coord, j - nstart, m->flex_interp[i]) > w) { + w = mju_evalBasis(coord, j - nstart, m->flex_interp[i]); + nodeid = j; + } + } + if (nodeid < 0) { + mjERROR("flex %d: node closest to vertex %d not found", i, vertid); + } + result.body = m->flex_nodebodyid[m->flex_nodeadr[i] + nodeid]; + + if (m->flex_centered[i]) { + mju_copy3(result.point, d->xpos + 3 * result.body); + } else { + mju_mulMatVec3(result.point, d->xmat + 9 * result.body, + m->flex_node + 3 * nodeid); + mju_addTo3(result.point, d->xpos + 3 * result.body); + } + } else { + result.body = m->flex_vertbodyid[m->flex_vertadr[i] + vertid]; + mju_copy3(result.point, + d->flexvert_xpos + 3 * (m->flex_vertadr[i] + vertid)); + } + result.flex = i; + } + return result; +} + +static void MakeSkin(const mjModel* m, const mjData* d, const mjvOption* opt, + int i, float* skinnormal, float* skinvert) { + int vertadr = m->skin_vertadr[i]; + int vertnum = m->skin_vertnum[i]; + int faceadr = m->skin_faceadr[i]; + int facenum = m->skin_facenum[i]; + + // accumulate positions from all bones + for (int j = m->skin_boneadr[i]; j < m->skin_boneadr[i] + m->skin_bonenum[i]; + j++) { + // get bind pose + mjtNum bindpos[3] = {(mjtNum)m->skin_bonebindpos[3 * j + 0], + (mjtNum)m->skin_bonebindpos[3 * j + 1], + (mjtNum)m->skin_bonebindpos[3 * j + 2]}; + mjtNum bindquat[4] = {(mjtNum)m->skin_bonebindquat[4 * j + 0], + (mjtNum)m->skin_bonebindquat[4 * j + 1], + (mjtNum)m->skin_bonebindquat[4 * j + 2], + (mjtNum)m->skin_bonebindquat[4 * j + 3]}; + + // compute rotation + int bodyid = m->skin_bonebodyid[j]; + mjtNum quat[4], quatneg[4], rotate[9]; + mju_negQuat(quatneg, bindquat); + mju_mulQuat(quat, d->xquat + 4 * bodyid, quatneg); + mju_quat2Mat(rotate, quat); + + // compute translation + mjtNum translate[3]; + mju_mulMatVec3(translate, rotate, bindpos); + mju_sub3(translate, d->xpos + 3 * bodyid, translate); + + // process all bone vertices + for (int k = m->skin_bonevertadr[j]; + k < m->skin_bonevertadr[j] + m->skin_bonevertnum[j]; k++) { + // vertex id and weight + int vid = m->skin_bonevertid[k]; + float vweight = m->skin_bonevertweight[k]; + + // get original position + mjtNum pos[3] = { + (mjtNum)m->skin_vert[3 * (vertadr + vid)], + (mjtNum)m->skin_vert[3 * (vertadr + vid) + 1], + (mjtNum)m->skin_vert[3 * (vertadr + vid) + 2], + }; + + // transform + mjtNum pos1[3]; + mju_mulMatVec3(pos1, rotate, pos); + mju_addTo3(pos1, translate); + + // accumulate position + skinvert[(3 * vid)] += vweight * (float)pos1[0]; + skinvert[(3 * vid) + 1] += vweight * (float)pos1[1]; + skinvert[(3 * vid) + 2] += vweight * (float)pos1[2]; + } + } + + // inflate + if (m->skin_inflate[i] && skinnormal != nullptr) { + // compute vertex normals from face normals + for (int k = faceadr; k < faceadr + facenum; k++) { + // get face vertex indices + int vid[3] = {m->skin_face[3 * k], m->skin_face[3 * k + 1], + m->skin_face[3 * k + 2]}; + + // get triangle edges + mjtNum vec01[3], vec02[3]; + for (int r = 0; r < 3; r++) { + vec01[r] = skinvert[3 * (vid[1]) + r] - skinvert[3 * (vid[0]) + r]; + vec02[r] = skinvert[3 * (vid[2]) + r] - skinvert[3 * (vid[0]) + r]; + } + + // compute face normal + mjtNum nrm[3]; + mju_cross(nrm, vec01, vec02); + + // add normal to each vertex with weight = area + for (int r = 0; r < 3; r++) { + for (int t = 0; t < 3; t++) { + skinnormal[3 * (vid[r]) + t] += nrm[t]; + } + } + } + + // normalize normals + for (int k = 0; k < vertnum; k++) { + float s = sqrtf(skinnormal[3 * (k) + 0] * skinnormal[3 * k + 0] + + skinnormal[3 * (k) + 1] * skinnormal[3 * k + 1] + + skinnormal[3 * (k) + 2] * skinnormal[3 * k + 2]); + + float scl = 1 / mjMAX(mjMINVAL, s); + skinnormal[3 * k] *= scl; + skinnormal[3 * k + 1] *= scl; + skinnormal[3 * k + 2] *= scl; + } + + float inflate = m->skin_inflate[i]; + for (int k = 0; k < vertnum; k++) { + skinvert[3 * k] += inflate * skinnormal[3 * k]; + skinvert[3 * k + 1] += inflate * skinnormal[3 * k + 1]; + skinvert[3 * k + 2] += inflate * skinnormal[3 * k + 2]; + } + } +} + +static PickResult PickSkin(const mjModel* m, const mjData* d, + const mjtNum ray_pos[3], const mjtNum ray_dir[3], + const mjvOption* vis_options) { + PickResult result; + if (!vis_options->flags[mjVIS_SKIN]) { + return result; + } + + std::vector vertex_buffer; + std::vector normal_buffer; + + for (int i = 0; i < m->nskin; i++) { + const int skin_group = mjMAX(0, mjMIN(mjNGROUP - 1, m->skin_group[i])); + if (!vis_options->skingroup[skin_group]) { + continue; + } + + vertex_buffer.resize(3 * m->skin_vertnum[i]); + if (m->skin_inflate[i]) { + normal_buffer.resize(3 * m->skin_vertnum[i]); + } + + float* skinvert = vertex_buffer.data(); + float* skinnormal = m->skin_inflate[i] ? normal_buffer.data() : nullptr; + MakeSkin(m, d, vis_options, i, skinvert, skinnormal); + + int vertid; + mjtNum test_dist = mju_raySkin(m->skin_facenum[i], m->skin_vertnum[i], + m->skin_face + 3 * m->skin_faceadr[i], + skinvert, ray_pos, ray_dir, &vertid); + if (test_dist < 0) { + continue; + } else if (result.dist >= 0 && test_dist >= result.dist) { + continue; + } + + result.dist = test_dist; + + // find body with largest weight for this vertex + float best_weight = -1; + for (int j = m->skin_boneadr[i]; + j < m->skin_boneadr[i] + m->skin_bonenum[i]; j++) { + for (int k = m->skin_bonevertadr[j]; + k < m->skin_bonevertadr[j] + m->skin_bonevertnum[j]; k++) { + // get vertex id and weight + const int vertex_id = m->skin_bonevertid[k]; + const float vertex_weight = m->skin_bonevertweight[k]; + + // update if matching id and bigger weight + if (vertex_id == vertid && vertex_weight > best_weight) { + best_weight = vertex_weight; + result.body = m->skin_bonebodyid[j]; + result.skin = i; + mju_f2n(result.point, skinvert + 3 * vertid, 3); + } + } + } + } + + return result; +} + +PickResult Pick(const mjModel* m, const mjData* d, const mjvCamera* camera, + float x, float y, float aspect_ratio, + const mjvOption* vis_options) { + mjtNum ray_pos[3]; + mjtNum ray_dir[3]; + MakePickRay(ray_pos, ray_dir, m, d, camera, x, 1.0 - y, aspect_ratio); + + PickResult results[3]; + results[0] = PickGeom(m, d, ray_pos, ray_dir, vis_options); + results[1] = PickFlex(m, d, ray_pos, ray_dir, vis_options); + results[2] = PickSkin(m, d, ray_pos, ray_dir, vis_options); + + PickResult best_result; + for (int i = 0; i < 3; i++) { + if (results[i].dist < 0) { + continue; + } + if (best_result.dist < 0 || results[i].dist < best_result.dist) { + best_result = results[i]; + } + } + return best_result; +} + int SetCamera(const mjModel* m, mjvCamera* camera, int request_idx) { // 0 = free, 1 = tracking, 2+ = fixed int camera_idx = std::clamp(request_idx, 0, std::max(m->ncam + 1, 0)); diff --git a/src/experimental/toolbox/interaction.h b/src/experimental/toolbox/interaction.h index 3da9039f..51f54cd2 100644 --- a/src/experimental/toolbox/interaction.h +++ b/src/experimental/toolbox/interaction.h @@ -21,7 +21,8 @@ namespace mujoco::toolbox { // The result of a pick operation. struct PickResult { - mjtNum point[3]; // World coordinates + mjtNum point[3] = {0, 0, 0}; // World coordinates + mjtNum dist = -1; // Distance from the camera. int body = -1; int geom = -1; int flex = -1; @@ -30,7 +31,7 @@ struct PickResult { // Returns information about the object (if any) under the mouse cursor. PickResult Pick(const mjModel* m, const mjData* d, const mjvCamera* camera, - float x, float y, float aspect_ratio, const mjvScene* scene, + float x, float y, float aspect_ratio, const mjvOption* vis_options); // Updates the camera according to the requested index using this convention: @@ -43,7 +44,18 @@ PickResult Pick(const mjModel* m, const mjData* d, const mjvCamera* camera, // convention. Note the returned index may differ from the request if the // request was invalid (index was out of range or tracking camera was not // available). -int SetCamera(const mjModel* model, mjvCamera* camera, int request_idx); +int SetCamera(const mjModel* m, mjvCamera* camera, int request_idx); + +// Moves the camera according to the mouse action and relative displacement. +void MoveCamera(const mjModel* m, const mjData* d, mjvCamera* cam, + mjtMouse action, mjtNum reldx, mjtNum reldy); + +void InitPerturb(const mjModel* m, const mjData* d, const mjvCamera* cam, + mjvPerturb* pert, mjtPertBit active); + +void MovePerturb(const mjModel* m, const mjData* d, const mjvCamera* cam, + mjvPerturb* pert, mjtMouse action, mjtNum reldx, + mjtNum reldy); } // namespace mujoco::toolbox