Rebuild scenes on terrain changes for dynamic heighfields

unity/Runtime/Components/Shapes/MjHeightFieldShape.cs

@@ -24,6 +24,7 @@ namespace Mujoco
 [Serializable]
 public class MjHeightFieldShape : IMjShape
 {
+  [Tooltip("Terrain's heightmap should have a minimum value of zero (fully black).")]
   public Terrain terrain;
 
   [Tooltip("The path, relative to Application.dataPath, where the heightmap data will be save in PNG format.")]
@@ -31,16 +32,41 @@ public class MjHeightFieldShape : IMjShape
 
   public string FullHeightMapPath => Path.GetFullPath(Path.Combine(Application.dataPath, heightMapExportPath));
   public int HeightMapWidth => terrain.terrainData.heightmapTexture.width; 
-  public int HeightMapHeight => terrain.terrainData.heightmapTexture.height;
+  public int HeightMapLength => terrain.terrainData.heightmapTexture.height;
   public Vector3 HeightMapScale => terrain.terrainData.heightmapScale;
 
-  public void ToMjcf(XmlElement mjcf, Transform transform){
+  [Tooltip("At least this many frames will have to pass before the scene is rebuilt with an updated heightmap. Leave as 0 to not update the hfield during simulation. Increasing this can improve performance.")]
+  public int UpdateLimit;
+  private int updateCountdown;
+
+  [HideInInspector]
+  public float MinimumHeight;
+  [HideInInspector]
+  public float MaximumHeight;
+
+  public int HeightFieldId { get; private set; }
+
+  public unsafe void ToMjcf(XmlElement mjcf, Transform transform){
     ExportHeightMap();
     if(terrain.transform.parent != transform) Debug.LogWarning($"The terrain of heightfield {transform.name} needs to be parented to the Geom for proper rendering.");
-    else terrain.transform.localPosition = new Vector3(-HeightMapHeight*HeightMapScale.x / 2, terrain.transform.localPosition.y, -HeightMapWidth*HeightMapScale.z / 2);
+    else {
+      if((terrain.transform.localPosition - new Vector3(-HeightMapLength * HeightMapScale.x / 2, terrain.transform.localPosition.y, -HeightMapWidth * HeightMapScale.z / 2)).magnitude > 0.001) {
+                    Debug.LogWarning($"Terrain of heightfield {transform.name} not aligned with geom. The terrain will be moved to accurately represent the simulated position.");
+      }
+      terrain.transform.localPosition = new Vector3(-HeightMapLength*HeightMapScale.x / 2, terrain.transform.localPosition.y, -HeightMapWidth*HeightMapScale.z / 2);
+    }
     var scene = MjScene.Instance;
     var assetName = scene.GenerationContext.AddHeightFieldAsset(this);
 
+    scene.postInitEvent += (_,_) => HeightFieldId = MujocoLib.mj_name2id(scene.Model, (int)MujocoLib.mjtObj.mjOBJ_HFIELD, assetName);
+
+    
+    if(UpdateLimit>0){
+      updateCountdown = UpdateLimit;
+      scene.preUpdateEvent += (_, _) => CountdownUpdateCondition();
+      TerrainCallbacks.heightmapChanged += RebuildScene;
+    }
+
     mjcf.SetAttribute("hfield", assetName);
   }
 
@@ -52,10 +78,24 @@ public class MjHeightFieldShape : IMjShape
     RenderTexture.active = terrain.terrainData.heightmapTexture;
     Texture2D texture = new Texture2D(RenderTexture.active.width, RenderTexture.active.height);
     texture.ReadPixels(new Rect(0, 0, RenderTexture.active.width, RenderTexture.active.height), 0, 0);
+    MaximumHeight = texture.GetPixels().Select(c => c.r).Max()*HeightMapScale.y*2;
+    var minimumHeight = texture.GetPixels().Select(c => c.r).Min()*HeightMapScale.y*2;
+    if (minimumHeight > 0.0001) Debug.LogWarning("Due to assumptions in MuJoCo heightfields, terrains should have a minimum heightmap value of 0.");
     RenderTexture.active = null;
     File.WriteAllBytes(FullHeightMapPath, texture.EncodeToPNG());
   }
 
+  public void CountdownUpdateCondition() {
+    if(updateCountdown < 1) return;
+    updateCountdown -= 1;
+  }
+
+  public void RebuildScene(Terrain terrain, RectInt heightRegion, bool synched){ 
+    if(updateCountdown > 0) return;
+    if(!Application.isPlaying || !MjScene.InstanceExists) return;
+    MjScene.Instance.SceneRecreationAtLateUpdateRequested = true;
+    updateCountdown = UpdateLimit;
+  }
 
   public Vector4 GetChangeStamp(){
     return Vector4.one;

unity/Runtime/Tools/MjcfGenerationContext.cs

@@ -115,13 +115,14 @@ public class MjcfGenerationContext {
     mjcf.SetAttribute("vertex", vertexPositionsStr.ToString());
   }
   
-  private static void GenerateHeightFieldMjcf(MjHeightFieldShape hFieldComponent, XmlElement mjcf) {
+   private static void GenerateHeightFieldMjcf(MjHeightFieldShape hFieldComponent, XmlElement mjcf) {
     mjcf.SetAttribute("nrow", "0");
     mjcf.SetAttribute("ncol", "0");
     mjcf.SetAttribute("content_type", "image/png");
     mjcf.SetAttribute("file", hFieldComponent.FullHeightMapPath);
-            
+    var baseHeight = hFieldComponent.terrain.transform.localPosition.y + hFieldComponent.MinimumHeight;
-    mjcf.SetAttribute("size", MjEngineTool.MakeLocaleInvariant($"{hFieldComponent.HeightMapScale.x*hFieldComponent.HeightMapHeight/2} {hFieldComponent.HeightMapScale.z * hFieldComponent.HeightMapWidth/2} {hFieldComponent.HeightMapScale.y} {hFieldComponent.terrain.transform.localPosition.y}"));
+    var heightRange = Mathf.Clamp(hFieldComponent.MaximumHeight - hFieldComponent.MinimumHeight, 0.00001f, Mathf.Infinity);
+    mjcf.SetAttribute("size", MjEngineTool.MakeLocaleInvariant($"{hFieldComponent.HeightMapScale.x*hFieldComponent.HeightMapLength/2} {hFieldComponent.HeightMapScale.z * hFieldComponent.HeightMapWidth/2} {heightRange} {baseHeight}"));
 
   }
 }