diff --git a/unity/Runtime/Components/Shapes/MjHeightFieldShape.cs b/unity/Runtime/Components/Shapes/MjHeightFieldShape.cs index 86b3fee9..390de7da 100644 --- a/unity/Runtime/Components/Shapes/MjHeightFieldShape.cs +++ b/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; diff --git a/unity/Runtime/Tools/MjcfGenerationContext.cs b/unity/Runtime/Tools/MjcfGenerationContext.cs index 86fb9ae7..9a6f3db6 100644 --- a/unity/Runtime/Tools/MjcfGenerationContext.cs +++ b/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); - - 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 baseHeight = hFieldComponent.terrain.transform.localPosition.y + hFieldComponent.MinimumHeight; + 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}")); } }