Mujoco C++ : Code pour injecter un plan de sol dans une scène

std::string inject_ground_plane(const std::string& xml) {
    // Tenter uniquement si cela ressemble à du MJCF
    if (xml.find("<mujoco") == std::string::npos) return {};

    // Léger soulèvement du plan visuel au-dessus de z=0 pour éviter le Z-fighting avec la
    // grille de débogage du visualiseur (qui est rendue à z=0). La physique est inchangée
    // à l'exception d'un décalage négligeable 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;

    // Tenter d'insérer dans le worldbody existant (juste avant sa balise de fermeture)
    size_t closeWorld = out.rfind("</worldbody>");
    if (closeWorld != std::string::npos) {
        out.insert(closeWorld, planeGeom);
        return out;
    }

    // Aucun worldbody trouvé : en créer un avant </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;
    }

    // En dernier recours, ajouter un wrapper minimal (ne devrait pas arriver pour du MJCF valide)
    return out + "\n<worldbody>" + planeGeom + "</worldbody>\n";
}