最小化 C++ MuJoCo 场景查看器：支持鼠标导航与仿真

// minimal_viewer.cpp
// 构建：见下方说明

#include <mujoco/mujoco.h>
#include <GLFW/glfw3.h>
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <time.h>

// --- 为 GLFW 回调保留的全局变量，保持精简 ---
static mjModel* m = nullptr;
static mjData*  d = nullptr;

static mjvCamera cam;       // 抽象相机
static mjvOption opt;       // 可视化选项
static mjvScene  scn;       // 抽象场景
static mjrContext con;      // 自定义 GPU 上下文

static bool  button_left = false, button_middle = false, button_right = false;
static double lastx = 0, lasty = 0;
// 实时同步：记录仿真和挂钟时间的起始时刻
static double sim_start_time = 0.0;
static double realtime_start = 0.0;

// --- GLFW 错误回调 ---
static void glfw_error_callback(int error, const char* description) {
  std::fprintf(stderr, "GLFW error %d: %s\n", error, description);
}

// --- 鼠标按键：记录哪些按键被按下 ---
static void mouse_button_callback(GLFWwindow* window, int button, int act, int /*mods*/) {
  // 简单、稳定的按键状态处理
  if (button == GLFW_MOUSE_BUTTON_LEFT)   button_left   = (act == GLFW_PRESS) ? true : (act == GLFW_RELEASE ? false : button_left);
  if (button == GLFW_MOUSE_BUTTON_MIDDLE) button_middle = (act == GLFW_PRESS) ? true : (act == GLFW_RELEASE ? false : button_middle);
  if (button == GLFW_MOUSE_BUTTON_RIGHT)  button_right  = (act == GLFW_PRESS) ? true : (act == GLFW_RELEASE ? false : button_right);

  // 记录当前光标位置（窗口坐标）
  glfwGetCursorPos(window, &lastx, &lasty);
}

// --- 鼠标移动：将运动转换为相机操作 ---
static void cursor_pos_callback(GLFWwindow* window, double xpos, double ypos) {
  if (!m) return;
  // 如果没有鼠标按键被按下，仅更新最后位置
  if (!button_left && !button_right && !button_middle) {
    lastx = xpos;
    lasty = ypos;
    return;
  }

  // 计算窗口坐标中的运动量，按帧缓冲高度归一化（在不同 DPI 下保持稳定）
  int fbw = 0, fbh = 1;
  glfwGetFramebufferSize(window, &fbw, &fbh);
  if (fbh <= 0) fbh = 1;

  double dx = (xpos - lastx) / (double)fbh;
  double dy = (ypos - lasty) / (double)fbh;
  lastx = xpos;
  lasty = ypos;

  if (button_left) {
    // 左键拖动：旋转
    mjv_moveCamera(m, mjMOUSE_ROTATE_H, dx, dy, &scn, &cam);
  } else if (button_right) {
    // 右键拖动：水平平移
    mjv_moveCamera(m, mjMOUSE_MOVE_H, dx, dy, &scn, &cam);
  } else if (button_middle) {
    // 中键拖动：缩放
    mjv_moveCamera(m, mjMOUSE_ZOOM, dx, dy, &scn, &cam);
  }
}

// --- 滚轮缩放 ---
static void scroll_callback(GLFWwindow* window, double /*xoffset*/, double yoffset) {
  if (!m) return;
  // 滚轮缩放（温和系数）
  mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05 * yoffset, &scn, &cam);
}

// --- 按键：'R' 重置 ---
static void key_callback(GLFWwindow* window, int key, int scancode, int act, int mods) {
  if (act == GLFW_PRESS || act == GLFW_REPEAT) {
    if (key == GLFW_KEY_R) {
      // 重置仿真状态和实时计时器，避免仿真跳到前面
      mj_resetData(m, d);
      sim_start_time = (d ? d->time : 0.0);
      realtime_start = glfwGetTime();
    }
    if (key == GLFW_KEY_ESCAPE) glfwSetWindowShouldClose(window, GLFW_TRUE);
  }
}

int main(int argc, char** argv) {
  // 用法
  if (argc < 2) {
    std::printf("Usage: %s model.xml\n", argv[0]);
    return 1;
  }

  // （开源 MuJoCo >= 2.2 无需许可证激活步骤）

  // 加载模型
  char error[1024] = {0};
  m = mj_loadXML(argv[1], nullptr, error, sizeof(error));
  if (!m) {
    std::fprintf(stderr, "Could not load model: %s\n", error);
    return 1;
  }
  d = mj_makeData(m);

  // 初始化 GLFW
  glfwSetErrorCallback(glfw_error_callback);
  if (!glfwInit()) {
    std::fprintf(stderr, "Could not initialize GLFW\n");
    return 1;
  }

  // 创建窗口/上下文
  glfwWindowHint(GLFW_DOUBLEBUFFER, 1);
  GLFWwindow* window = glfwCreateWindow(1200, 900, "MuJoCo Minimal Viewer", nullptr, nullptr);
  if (!window) {
    std::fprintf(stderr, "Could not create GLFW window\n");
    glfwTerminate();
    return 1;
  }
  glfwMakeContextCurrent(window);
  glfwSwapInterval(0); // 禁用 vsync

  // 设置回调
  glfwSetMouseButtonCallback(window, mouse_button_callback);
  glfwSetCursorPosCallback(window, cursor_pos_callback);
  glfwSetScrollCallback(window, scroll_callback);
  glfwSetKeyCallback(window, key_callback);

  // 初始化 MuJoCo 可视化
  mjv_defaultCamera(&cam);
  // 设置相机注视点，使初始视图居中于模型
  cam.lookat[0] = 0.0;
  cam.lookat[1] = 0.0;
  cam.lookat[2] = 0.5;
  mjv_defaultOption(&opt);
  mjv_defaultScene(&scn);
  mjr_defaultContext(&con);

  mjv_makeScene(m, &scn, 2000);               // 分配场景
  mjr_makeContext(m, &con, mjFONTSCALE_100);  // 分配 GPU 上下文

  // 初始化实时同步计时器（两者在同一挂钟时间开始）
  sim_start_time = d->time;
  realtime_start = glfwGetTime();

  // 放置相机以显示整个模型
  cam.type = mjCAMERA_FREE;
  mj_forward(m, d);

  // 主循环
  while (!glfwWindowShouldClose(window)) {
    // 推进仿真（简单的近似实时步进）
    mj_step(m, d);

    // --- 实时同步：确保仿真不会比挂钟更快 ---
    double sim_elapsed = d->time - sim_start_time;                 // 自开始/重置以来的仿真秒数
    double real_elapsed = glfwGetTime() - realtime_start;          // 自开始/重置以来的挂钟秒数
    double sleep_time = sim_elapsed - real_elapsed;                // 为正 => 仿真超前
    if (sleep_time > 0.0) {
      // 休眠必要的时间（nanosleep 无需线程链接器标志即可安全使用）
      struct timespec req;
      req.tv_sec = (time_t)std::floor(sleep_time);
      req.tv_nsec = (long)((sleep_time - req.tv_sec) * 1e9);
      if (req.tv_sec < 0) req.tv_sec = 0;
      if (req.tv_nsec < 0) req.tv_nsec = 0;
      nanosleep(&req, nullptr);
    }

    // 获取帧缓冲大小并设置视口
    int width, height;
    glfwGetFramebufferSize(window, &width, &height);
    mjrRect viewport = {0, 0, width, height};

    // 更新场景并渲染
    mjv_updateScene(m, d, &opt, nullptr, &cam, mjCAT_ALL, &scn);
    mjr_render(viewport, &scn, &con);

    // 交换缓冲并轮询事件
    glfwSwapBuffers(window);
    glfwPollEvents();
  }

  // 清理
  mjr_freeContext(&con);
  mjv_freeScene(&scn);
  mj_deleteData(d);
  mj_deleteModel(m);
  glfwTerminate();
  return 0;
}

cmake_minimum_required(VERSION 3.10)
project(mujocotest)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

find_package(PkgConfig REQUIRED)
pkg_search_module(MUJOCO mujoco)
pkg_search_module(GLFW REQUIRED glfw3)

if(NOT MUJOCO_FOUND)
	set(MUJOCO_INCLUDE_DIRS /opt/mujoco/include)
	set(MUJOCO_LIBRARIES /opt/mujoco/lib/libmujoco.so)
endif()

add_executable(mujocotest main.cpp)
target_include_directories(mujocotest PRIVATE ${MUJOCO_INCLUDE_DIRS} ${GLFW_INCLUDE_DIRS})
target_link_libraries(mujocotest PRIVATE ${MUJOCO_LIBRARIES} ${GLFW_LIBRARIES})

cmake .
make -j8

./mujocotest ~/mujoco_menagerie/franka_fr3/fr3.xml