Mujoco C++-Code zum Injizieren einer Bodenebene in eine Szene

std::string inject_ground_plane(const std::string& xml) {
    // Nur versuchen, wenn es nach MJCF aussieht
    if (xml.find("<mujoco") == std::string::npos) return {};

    // Hebe die visuelle Ebene leicht über z=0 an, um Z-fighting mit dem
    // Debug-Raster des Viewers (das bei z=0 gerendert wird) zu vermeiden.
    // Die Physik bleibt unverändert, abgesehen von einem vernachlässigbaren
    // 1mm-Versatz.
    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;

    // Versuche, in eine bestehende worldbody einzufügen (direkt vor ihrem schließenden Tag)
    size_t closeWorld = out.rfind("</worldbody>");
    if (closeWorld != std::string::npos) {
        out.insert(closeWorld, planeGeom);
        return out;
    }

    // Keine worldbody gefunden: Erstelle eine vor </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;
    }

    // Als letzten Ausweg: einen minimalen Wrapper anhängen (sollte bei gültigem MJCF nicht vorkommen)
    return out + "\n<worldbody>" + planeGeom + "</worldbody>\n";
}