Código Mujoco C++ para injetar um plano de chão em uma cena

std::string inject_ground_plane(const std::string& xml) {
    // Apenas tentar se parecer MJCF
    if (xml.find("<mujoco") == std::string::npos) return {};

    // Levantar ligeiramente o plano visual acima de z=0 para evitar Z-fighting com a
    // grade de debug do viewer (que é renderizada em z=0). A física não muda
    // exceto por um deslocamento negligenciável de 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;

    // Tentar inserir no worldbody existente (logo antes de sua tag de fechamento)
    size_t closeWorld = out.rfind("</worldbody>");
    if (closeWorld != std::string::npos) {
        out.insert(closeWorld, planeGeom);
        return out;
    }

    // Nenhum worldbody encontrado: criar um antes de </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;
    }

    // Como último recurso, anexar um wrapper minimal (não deveria acontecer para MJCF válido)
    return out + "\n<worldbody>" + planeGeom + "</worldbody>\n";
}