Merge pull request #123 from Algoryx/feature/set-get-terrain-heights

Add setters and getters for terrain heights

AGXUnity/Model/DeformableTerrain.cs

@@ -319,6 +319,79 @@ public override void ConvertToDynamicMassInShape( Shape failureVolume )
         Native.convertToDynamicMassInShape( failureVolume.GetInitialized<Shape>().NativeShape );
     }
 
+    public override void SetHeights( int xstart, int ystart, float[,] heights )
+    {
+      int height = heights.GetLength(0);
+      int width = heights.GetLength(1);
+      int resolution = TerrainDataResolution;
+
+      if ( xstart + width >= resolution || xstart < 0 || ystart + height >= resolution || ystart < 0 )
+        throw new ArgumentOutOfRangeException( "", $"Provided height patch with start ({xstart},{ystart}) and size ({width},{height}) extends outside of the terrain bounds [0,{TerrainDataResolution - 1}]" );
+
+      float scale = TerrainData.size.y;
+      float depthOffset = 0;
+      if ( Native != null )
+        depthOffset = MaximumDepth;
+
+      for ( int y = 0; y < height; y++ ) {
+        for ( int x = 0; x < width; x++ ) {
+          float value = heights[ y, x ] + depthOffset;
+          heights[ y, x ] = value / scale;
+
+          agx.Vec2i idx = new agx.Vec2i( resolution - 1 - x - xstart, resolution - 1 - y - ystart);
+          Native?.setHeight( idx, value );
+        }
+      }
+
+      TerrainData.SetHeights( xstart, ystart, heights );
+    }
+    public override void SetHeight( int x, int y, float height )
+    {
+      if ( x >= TerrainDataResolution || x < 0 || y >= TerrainDataResolution || y < 0 )
+        throw new ArgumentOutOfRangeException( "(x, y)", $"Indices ({x},{y}) is outside of the terrain bounds [0,{TerrainDataResolution - 1}]" );
+
+      if ( Native != null )
+        height += MaximumDepth;
+
+      agx.Vec2i idx = new agx.Vec2i( TerrainDataResolution - 1 - x, TerrainDataResolution - 1 - y );
+      Native?.setHeight( idx, height );
+
+      TerrainData.SetHeights( x, y, new float[,] { { height / TerrainData.size.y } } );
+    }
+    public override float[,] GetHeights( int xstart, int ystart, int width, int height )
+    {
+      if ( width <= 0 || height <= 0 )
+        throw new ArgumentOutOfRangeException( "width, height", $"Width and height ({width} / {height}) must be greater than 0" );
+
+      int resolution = TerrainDataResolution;
+
+      if ( xstart + width >= resolution || xstart < 0 || ystart + height >= resolution || ystart < 0 )
+        throw new ArgumentOutOfRangeException( "", $"Requested height patch with start ({xstart},{ystart}) and size ({width},{height}) extends outside of the terrain bounds [0,{TerrainDataResolution - 1}]" );
+
+      if ( Native == null )
+        return TerrainData.GetHeights( xstart, ystart, width, height );
+
+      float [,] heights = new float[height,width];
+      for ( int y = 0; y < height; y++ ) {
+        for ( int x = 0; x < width; x++ ) {
+          agx.Vec2i idx = new agx.Vec2i( resolution - 1 - x - xstart, resolution - 1 - y - ystart);
+          heights[ y, x ] = (float)Native.getHeight( idx ) - MaximumDepth;
+        }
+      }
+      return heights;
+    }
+    public override float GetHeight( int x, int y )
+    {
+      if ( x >= TerrainDataResolution || x < 0 || y >= TerrainDataResolution || y < 0 )
+        throw new ArgumentOutOfRangeException( "(x, y)", $"Indices ({x},{y}) is outside of the terrain bounds [0,{TerrainDataResolution - 1}]" );
+
+      if ( Native == null )
+        return TerrainData.GetHeight( x, y );
+
+      agx.Vec2i idx = new agx.Vec2i( TerrainDataResolution - 1 - x, TerrainDataResolution - 1 - y );
+      return (float)Native.getHeight( idx ) - MaximumDepth;
+    }
+
     protected override bool IsNativeNull() { return Native == null; }
     protected override void SetShapeMaterial( agx.Material material, agxTerrain.Terrain.MaterialType type ) { Native.setMaterial( material, type ); }
     protected override void SetTerrainMaterial( agxTerrain.TerrainMaterial material ) { Native.setTerrainMaterial( material ); }

AGXUnity/Model/DeformableTerrainBase.cs

@@ -196,14 +196,54 @@ virtual public void OnPropertiesUpdated() { }
     /// Verifies so that all added shovels still exists. Shovels that
     /// has been deleted are removed.
     /// </summary>
-    abstract public void RemoveInvalidShovels( bool removeDisabled = false, bool warn = false);
+    abstract public void RemoveInvalidShovels( bool removeDisabled = false, bool warn = false );
 
     /// <summary>
     /// Converts any part of the terrain that overlaps the provided shape into dynamic mass
     /// </summary>
     /// <param name="failureVolume">The shape in which to convert the terrain into dynamic mass</param>
     abstract public void ConvertToDynamicMassInShape( Collide.Shape failureVolume );
 
+    /// <summary>
+    /// Sets the heights in the terrain starting at the specified terrain indices using Unity's indexing convention.
+    /// This function is meant to mimic <see cref="UnityEngine.TerrainData.SetHeights"/>.
+    /// The width and height of the terrain patch which is set is inferred from the size of the provided heights array.
+    /// </summary>
+    /// <param name="xstart">The x-index at which to start setting the heights.</param>
+    /// <param name="ystart">The y-index at which to start setting the heights.</param>
+    /// <param name="heights">The heights to write into the terrain.</param>
+    abstract public void SetHeights( int xstart, int ystart, float[,] heights );
+
+    /// <summary>
+    /// Sets the height in the terrain at the specified terrain index using Unity's indexing convention.
+    /// This function is meant to mimic <see cref="UnityEngine.TerrainData.SetHeights"/>.
+    /// </summary>
+    /// <param name="x">The x-index at which to set the height.</param>
+    /// <param name="y">The y-index at which to set the height.</param>
+    /// <param name="height">The height to write into the terrain.</param>
+    abstract public void SetHeight( int x, int y, float height );
+
+    /// <summary>
+    /// Gets the heights in the terrain patch starting at the specified terrain indices using Unity's indexing convention
+    /// and covering the specified width and height.
+    /// This function is meant to mimic <see cref="UnityEngine.TerrainData.GetHeights"/>.
+    /// </summary>
+    /// <param name="xstart">The x-index at which to start getting the heights.</param>
+    /// <param name="ystart">The y-index at which to start getting the heights.</param>
+    /// <param name="width">The width of the region for which to get the heights.</param>
+    /// <param name="height">The height of the region for which to get the heights.</param>
+    /// <returns>A 2D array with the specified width and height containing the heights of the terrain in the specified patch.</returns>
+    abstract public float[,] GetHeights( int xstart, int ystart, int width, int height );
+
+    /// <summary>
+    /// Gets the heights in the terrain patch starting at the specified terrain index using Unity's indexing convention.
+    /// This function is meant to mimic <see cref="UnityEngine.TerrainData.GetHeight"/>.
+    /// </summary>
+    /// <param name="x">The x-index at which to get the height.</param>
+    /// <param name="y">The y-index at which to get the height.</param>
+    /// <returns>The height of the terrain at the specified index.</returns>
+    abstract public float GetHeight( int x, int y );
+
     abstract protected bool IsNativeNull();
     abstract protected void SetShapeMaterial( agx.Material material, agxTerrain.Terrain.MaterialType type );
     abstract protected void SetTerrainMaterial( agxTerrain.TerrainMaterial material );

AGXUnity/Model/DeformableTerrainPager.cs

@@ -528,7 +528,7 @@ public override void RemoveInvalidShovels( bool removeDisabled = false, bool war
       if ( removeDisabled ) {
         int remShovels = m_shovels.RemoveAll( shovel => !shovel.Body.isActiveAndEnabled );
         int remRBs = m_rigidbodies.RemoveAll( rb => !rb.Body.isActiveAndEnabled );
-        if(remShovels + remRBs > 0 ) {
+        if ( remShovels + remRBs > 0 ) {
           if ( warn )
             Debug.LogWarning( $"Removed {remShovels} disabled shovels and {remRBs} disabled rigid bodies from terrain {gameObject.name}." +
                               " Disabled objects should not be added to the terrain on play and should instead be added manually when enabled during runtime." +
@@ -548,6 +548,208 @@ public override void ConvertToDynamicMassInShape( Shape failureVolume )
         shape.ReturnToPool();
       }
     }
+
+    public override void SetHeights( int xstart, int ystart, float[,] heights )
+    {
+      if ( Native == null )
+        throw new NotSupportedException( "Calling SetHeights on an uninitialized DeformableTerrainPager is not supported" );
+      SetHeightsInternal( xstart, ystart, heights, false );
+    }
+
+    private void SetHeightsInternal( int xstart, int ystart, float[,] heights, bool recursive )
+    {
+      int height = heights.GetLength(0);
+      int width = heights.GetLength(1);
+
+      agx.Vec2i idx = new agx.Vec2i( xstart, ystart );
+
+      // General process for setting terrain pager heights uses a recursive approach.
+      // if we can find a pager tile which overlaps the lowest x/y index we can divide the whole patch into three subpatches:
+      // 1. The patch which is the intersection between the whole patch and the found tile.
+      // 2. The patch from the highest x index in the found tile to the highest x index in the whole patch
+      // 3. The remaining patch.
+      // ____________
+      // |   |   3  |
+      // |   |______|
+      // | 2 |   1  |
+      // |___|______|
+
+      // For (1) we can set the heights directly from the tile which contains it and for (2) & (3) we can recursively apply the same
+      // procedure to set the heights.
+      bool tileFound = false;
+      var tiles = Native.getActiveTileAttachments();
+      foreach ( var tile in tiles ) {
+        var gi = GetGlobalIndex( tile.m_terrainTile, new agx.Vec2i( 0, 0 ) );
+        if ( gi.x > idx.x || gi.y > idx.y || gi.x + TileSize <= idx.x || gi.y + TileSize <= idx.y )
+          continue;
+
+        var tileStart = new agx.Vec2i(idx.x - gi.x, idx.y - gi.y);
+        var size = new agx.Vec2i32(Mathf.Min(width, TileSize - (int)tileStart.x), Mathf.Min(height,TileSize - (int)tileStart.y));
+
+        // TODO: Use 2D array segments instead of creating new arrays for each tile
+        // Set heights for (1) directly using the tile
+        float[,] thisTile = new float[size.y,size.x];
+        for ( int y = 0; y < size.y; y++ )
+          for ( int x = 0; x < size.x; x++ )
+            thisTile[ y, x ] = heights[ y, x ];
+        TerrainSetHeights( tile, (int)tileStart.x, (int)tileStart.y, thisTile );
+
+        // Set heights for (2) recursively
+        if ( size.x != width ) {
+          float[,] sideTile = new float[height,width - size.x];
+          for ( int y = 0; y < height; y++ )
+            for ( int x = 0; x < width - size.x; x++ )
+              sideTile[ y, x ] = heights[ y, x + size.x ];
+          SetHeightsInternal( xstart + size.x, ystart, sideTile, true );
+        }
+
+        // Set heights for (3) recursively
+        if ( size.y != height ) {
+          float[,] topTile = new float[height - size.y,size.x];
+          for ( int y = 0; y < height - size.y; y++ )
+            for ( int x = 0; x < size.x; x++ )
+              topTile[ y, x ] = heights[ y + size.y, x ];
+          SetHeightsInternal( xstart, ystart + size.y, topTile, true );
+        }
+
+        tileFound = true;
+        break;
+      }
+
+      if ( !tileFound )
+        throw new ArgumentOutOfRangeException( "", $"Terrain patch at ({xstart}, {ystart}) with size [{width},{height}] contains inactive tiles" );
+
+      if ( !recursive )
+        UpdateHeights();
+    }
+    public override void SetHeight( int x, int y, float height )
+    {
+      if ( Native == null )
+        throw new NotSupportedException( "Calling SetHeight on an uninitialized DeformableTerrainPager is not supported" );
+
+      agx.Vec2i idx = new agx.Vec2i( x, y );
+
+      var tiles = Native.getActiveTileAttachments();
+      foreach ( var tile in tiles ) {
+        var gi = GetGlobalIndex( tile.m_terrainTile, new agx.Vec2i( 0, 0 ) );
+        if ( gi.x > idx.x || gi.y > idx.y || gi.x + TileSize <= idx.x || gi.y + TileSize <= idx.y )
+          continue;
+
+        tile.m_terrainTile.setHeight( new agx.Vec2i( idx.x - gi.x, idx.y - gi.y ), height - (float)tile.m_zOffset + MaximumDepth );
+        UpdateHeights();
+        return;
+      }
+      throw new ArgumentOutOfRangeException( "(x, y)", $"No active tile corresponds to the global index ({x}, {y})" );
+    }
+    public override float[,] GetHeights( int xstart, int ystart, int width, int height )
+    {
+      if ( Native == null )
+        throw new NotSupportedException( "Calling GetHeights on an uninitialized DeformableTerrainPager is not supported" );
+
+      if ( width <= 0 || height <= 0 )
+        throw new ArgumentOutOfRangeException( "width, height", $"Width and height ({width} / {height}) must be greater than 0" );
+      float [,] heights = new float[height,width];
+      agx.Vec2i idx = new agx.Vec2i( xstart, ystart );
+
+      // General process for setting terrain pager heights uses a recursive approach.
+      // if we can find a pager tile which overlaps the lowest x/y index we can divide the whole patch into three subpatches:
+      // 1. The patch which is the intersection between the whole patch and the found tile.
+      // 2. The patch from the highest x index in the found tile to the highest x index in the whole patch
+      // 3. The remaining patch.
+      // ____________
+      // |   |   3  |
+      // |   |______|
+      // | 2 |   1  |
+      // |___|______|
+
+      // For (1) we can get the heights directly from the tile which contains it and for (2) & (3) we can recursively apply the same
+      // procedure to get the heights.
+      // When the height arrays for (1), (2) & (3) are all filled we can combine them to yield the total height array.
+      var tiles = Native.getActiveTileAttachments();
+      foreach ( var tile in tiles ) {
+        var gi = GetGlobalIndex( tile.m_terrainTile, new agx.Vec2i( 0, 0 ) );
+        if ( gi.x > idx.x || gi.y > idx.y || gi.x + TileSize <= idx.x || gi.y + TileSize <= idx.y )
+          continue;
+
+        var tileStart = new agx.Vec2i(idx.x - gi.x, idx.y - gi.y);
+        var size = new agx.Vec2i32(Mathf.Min(width, TileSize - (int)tileStart.x), Mathf.Min(height,TileSize - (int)tileStart.y));
+
+        // TODO: Use 2D array segments instead of creating new arrays for each tile
+        // Get heights for (1) directly from tile
+        float[,] thisTile = TerrainGetHeights(tile, (int)tileStart.x, (int)tileStart.y, size.x, size.y);
+        float[,] sideTile = null;
+        float[,] topTile = null;
+        // Get heights for (2) recursively
+        if ( size.x != width )
+          sideTile = GetHeights( xstart + size.x, ystart, width - size.x, height );
+        // Get heights for (3) recursively
+        if ( size.y != height )
+          topTile = GetHeights( xstart, ystart + size.y, size.x, height - size.y );
+
+        // Combine height-arrays for (1),(2) & (3)
+        for ( int y = 0; y < thisTile.GetLength( 0 ); y++ )
+          for ( int x = 0; x < thisTile.GetLength( 1 ); x++ )
+            heights[ y, x ] = thisTile[ y, x ];
+
+        if ( sideTile != null )
+          for ( int y = 0; y < sideTile.GetLength( 0 ); y++ )
+            for ( int x = 0; x < sideTile.GetLength( 1 ); x++ )
+              heights[ y, x + size.x ] = sideTile[ y, x ];
+
+        if ( topTile != null )
+          for ( int y = 0; y < topTile.GetLength( 0 ); y++ )
+            for ( int x = 0; x < topTile.GetLength( 1 ); x++ )
+              heights[ y + size.y, x ] = topTile[ y, x ];
+
+        return heights;
+      }
+
+      throw new ArgumentOutOfRangeException( "", $"Terrain patch at ({xstart}, {ystart}) with size [{width},{height}] contains inactive tiles" );
+    }
+    public override float GetHeight( int x, int y )
+    {
+      if ( Native == null )
+        throw new NotSupportedException( "Calling GetHeight on an uninitialized DeformableTerrainPager is not supported" );
+
+      agx.Vec2i idx = new agx.Vec2i( x, y );
+
+      var tiles = Native.getActiveTileAttachments();
+      foreach ( var tile in tiles ) {
+        var gi = GetGlobalIndex( tile.m_terrainTile, new agx.Vec2i( 0, 0 ) );
+        if ( gi.x > idx.x || gi.y > idx.y || gi.x + TileSize <= idx.x || gi.y + TileSize <= idx.y )
+          continue;
+
+        return (float)tile.m_terrainTile.getHeight( new agx.Vec2i( idx.x - gi.x, idx.y - gi.y ) ) + (float)tile.m_zOffset - MaximumDepth;
+      }
+      throw new ArgumentOutOfRangeException( "(x, y)", $"No active tile corresponds to the global index ({x}, {y})" );
+    }
+
+    private float[,] TerrainGetHeights( agxTerrain.TerrainPager.TileAttachments tile, int xstart, int ystart, int width, int height )
+    {
+      var terr = tile.m_terrainTile;
+      var offset = tile.m_zOffset;
+      float [,] heights = new float[height,width];
+      for ( int y = 0; y < height; y++ ) {
+        for ( int x = 0; x < width; x++ ) {
+          agx.Vec2i idx = new agx.Vec2i(xstart + x, ystart + y);
+          heights[ y, x ] = (float)terr.getHeight( idx ) + (float)offset - MaximumDepth;
+        }
+      }
+      return heights;
+    }
+
+    private void TerrainSetHeights( agxTerrain.TerrainPager.TileAttachments tile, int xstart, int ystart, float[,] heights )
+    {
+      var terr = tile.m_terrainTile;
+      var offset = tile.m_zOffset;
+      for ( int y = 0; y < heights.GetLength( 0 ); y++ ) {
+        for ( int x = 0; x < heights.GetLength( 1 ); x++ ) {
+          agx.Vec2i idx = new agx.Vec2i(xstart + x, ystart + y);
+          terr.setHeight( idx, heights[ y, x ] - offset + MaximumDepth );
+        }
+      }
+    }
+
     protected override bool IsNativeNull() { return Native == null; }
     protected override void SetShapeMaterial( agx.Material material, agxTerrain.Terrain.MaterialType type )
     {