Mujoco C++：向场景注入地平面的代码

std::string inject_ground_plane(const std::string& xml) {
    // 仅在看起来像 MJCF 时尝试
    if (xml.find("<mujoco") == std::string::npos) return {};

    // 将视觉平面略微提升到 z=0 以上，以避免与
    // viewer 的调试网格（在 z=0 渲染）发生 Z-fighting。物理特性不变，
    // 仅有可忽略的 1mm 偏移。
    std::string planeGeom =
        "\n    <geom name=\"ground\" type=\"plane\" pos=\"0 0 0.001\" size=\"0 0 1\" "
        "rgba=\"0.2 0.2 0.8 0.5\" contype=\"1\" conaffinity=\"1\"/\>\n";

    std::string out = xml;

    // 尝试插入现有的 worldbody（在其闭合标签之前）
    size_t closeWorld = out.rfind("</worldbody>");
    if (closeWorld != std::string::npos) {
        out.insert(closeWorld, planeGeom);
        return out;
    }

    // 未找到 worldbody：在 </mujoco> 之前创建一个
    size_t closeMujoco = out.rfind("</mujoco>");
    if (closeMujoco != std::string::npos) {
        std::string block = std::string("\n  <worldbody>\n") + planeGeom + "  </worldbody>\n";
        out.insert(closeMujoco, block);
        return out;
    }

    // 最后手段：追加一个最小包装器（对于有效的 MJCF 不应发生）
    return out + "\n<worldbody>" + planeGeom + "</worldbody>\n";
}