├── .gitignore ├── LICENSE ├── README.md ├── doc ├── confined-terrain-generation.gif ├── different-terrains.gif └── tiling.gif ├── environment.yaml ├── examples ├── configs │ ├── indoor_cfg.py │ ├── navigation_cfg.py │ └── overhanging_cfg.py ├── create_primitive_course.py ├── generate_mountain.py ├── generate_with_wfc.py ├── visualize_history.py └── visualize_sdf.py ├── setup.py └── terrain_generator ├── __init__.py ├── navigation ├── graph_search.py └── mesh_terrain.py ├── test ├── __init__.py ├── conftest.py ├── test_basic_parts.py ├── test_mesh.py ├── test_mesh_terrain.py ├── test_nav_utils.py ├── test_overhanging.py ├── test_tiles.py ├── test_utils.py └── test_wfc.py ├── trimesh_tiles ├── __init__.py ├── mesh_parts │ ├── __init__.py │ ├── basic_parts.py │ ├── create_tiles.py │ ├── indoor_parts.py │ ├── mesh_parts_cfg.py │ ├── mountain.py │ ├── overhanging_parts.py │ ├── rough_parts.py │ └── tree.py ├── patterns │ ├── overhanging_patterns.py │ └── pattern_generator.py └── primitive_course │ └── steps.py ├── utils ├── __init__.py ├── mesh_utils.py ├── nav_utils.py └── utils.py └── wfc ├── __init__.py ├── tiles.py └── wfc.py /.gitignore: -------------------------------------------------------------------------------- 1 | results/ 2 | mesh_cache 3 | *.pyc 4 | *.pkl 5 | *.obj 6 | *.stl 7 | *.json 8 | 9 | terrain_generator.egg-info -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 ETH Zurich, Takahiro Miki 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Terrain Generator 2 | This is a automated terrain geneartor tool using [wave function collapse](https://github.com/mxgmn/WaveFunctionCollapse) method. 3 | 4 | This is used in the paper, [Learning to walk in confined spaces using 3D representation](https://takahiromiki.com/publication-posts/learning-to-walk-in-confined-spaces-using-3d-representation/) 5 | 6 | [Project page](https://takahiromiki.com/publication-posts/learning-to-walk-in-confined-spaces-using-3d-representation/), [arxiv](https://arxiv.org/abs/2403.00187), [Youtube](https://youtu.be/QAwBoN55p9I) 7 | 8 | ![Confined Terrain Generation](doc/confined-terrain-generation.gif) 9 | 10 | ## Tiling meshes 11 | It checks the connectivity of each mesh parts and connect them. 12 |

13 | 14 | 15 |

16 | 17 | 18 | # Install 19 | If you're using conda, create env with the following command. 20 | ```bash 21 | conda env create -f environment.yaml 22 | pip install -e . 23 | ``` 24 | 25 | # Usage 26 | To run a testing script run as follows. 27 | ```bash 28 | conda activate wfc 29 | python3 examples/generate_with_wfc.py 30 | ``` 31 | This will first generate all configured meshes and then build different combinations. 32 | Once the mesh is generated, it is stored as cache and reused for the next time. 33 | 34 | You can make your own config to generate different terrians. 35 | 36 | # Citation 37 | Please cite the following paper if you use this software. 38 | 39 | ``` 40 | @article{miki2024learning, 41 | title={Learning to walk in confined spaces using 3D representation}, 42 | author={Miki, Takahiro and Lee, Joonho and Wellhausen, Lorenz and Hutter, Marco}, 43 | journal={arXiv preprint arXiv:2403.00187}, 44 | year={2024} 45 | } 46 | ``` 47 | 48 | # Config 49 | TODO 50 | -------------------------------------------------------------------------------- /doc/confined-terrain-generation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/terrain-generator/7be9d9a6ebbed7dc669486426184a4694bfd0bb6/doc/confined-terrain-generation.gif -------------------------------------------------------------------------------- /doc/different-terrains.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/terrain-generator/7be9d9a6ebbed7dc669486426184a4694bfd0bb6/doc/different-terrains.gif -------------------------------------------------------------------------------- /doc/tiling.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/terrain-generator/7be9d9a6ebbed7dc669486426184a4694bfd0bb6/doc/tiling.gif -------------------------------------------------------------------------------- /environment.yaml: -------------------------------------------------------------------------------- 1 | name: wfc 2 | channels: 3 | - defaults 4 | dependencies: 5 | - _libgcc_mutex=0.1=main 6 | - _openmp_mutex=5.1=1_gnu 7 | - ca-certificates=2022.10.11=h06a4308_0 8 | - certifi=2022.9.24=py38h06a4308_0 9 | - ld_impl_linux-64=2.38=h1181459_1 10 | - libffi=3.4.2=h6a678d5_6 11 | - libgcc-ng=11.2.0=h1234567_1 12 | - libgomp=11.2.0=h1234567_1 13 | - libstdcxx-ng=11.2.0=h1234567_1 14 | - ncurses=6.3=h5eee18b_3 15 | - openssl=1.1.1s=h7f8727e_0 16 | - pip=22.3.1=py38h06a4308_0 17 | - python=3.8.15=h7a1cb2a_2 18 | - readline=8.2=h5eee18b_0 19 | - setuptools=65.5.0=py38h06a4308_0 20 | - sqlite=3.40.0=h5082296_0 21 | - tk=8.6.12=h1ccaba5_0 22 | - wheel=0.37.1=pyhd3eb1b0_0 23 | - xz=5.2.8=h5eee18b_0 24 | - zlib=1.2.13=h5eee18b_0 25 | - pip: 26 | - about-time==4.2.1 27 | - alive-progress==3.0.1 28 | - attrs==22.2.0 29 | - chardet==5.1.0 30 | - colorlog==6.7.0 31 | - contourpy==1.0.6 32 | - distlib==0.3.6 33 | - exceptiongroup==1.1.0 34 | - future==0.18.2 35 | - grapheme==0.6.0 36 | - hilbertcurve==2.0.5 37 | - idna==3.4 38 | - imageio==2.22.4 39 | - iniconfig==1.1.1 40 | - lxml==4.9.2 41 | - mapbox-earcut==1.0.1 42 | - matplotlib==3.6.2 43 | - mpmath==1.2.1 44 | - networkx==2.8.8 45 | - opencv-python==4.7.0.68 46 | - ortools==9.5.2237 47 | - git+https://github.com/pvigier/perlin-numpy 48 | - pillow==9.3.0 49 | - protobuf==4.21.12 50 | - pycollada==0.7.2 51 | - pyglet==1.5.0 52 | - pyparsing==3.0.9 53 | - pytest==7.2.0 54 | - ray==2.2.0 55 | - requests==2.28.1 56 | - rtree==1.0.1 57 | - six==1.16.0 58 | - svg-path==6.2 59 | - sympy==1.11.1 60 | - trimesh==3.17.1 61 | - urllib3==1.26.13 62 | - virtualenv==20.17.1 63 | - xxhash==3.2.0 64 | -------------------------------------------------------------------------------- /examples/configs/indoor_cfg.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | from typing import Tuple, Optional 7 | from dataclasses import dataclass 8 | 9 | from numpy.random import f 10 | 11 | from terrain_generator.trimesh_tiles.mesh_parts.mesh_parts_cfg import ( 12 | MeshPattern, 13 | MeshPartsCfg, 14 | WallPartsCfg, 15 | StairMeshPartsCfg, 16 | PlatformMeshPartsCfg, 17 | HeightMapMeshPartsCfg, 18 | ) 19 | from terrain_generator.trimesh_tiles.mesh_parts.rough_parts import generate_perlin_tile_configs 20 | 21 | from terrain_generator.trimesh_tiles.patterns.pattern_generator import ( 22 | generate_walls, 23 | generate_floating_boxes, 24 | generate_narrow, 25 | generate_platforms, 26 | generate_ramp_parts, 27 | generate_stair_parts, 28 | generate_stepping_stones, 29 | ) 30 | from terrain_generator.trimesh_tiles.mesh_parts.create_tiles import create_mesh_tile 31 | from terrain_generator.trimesh_tiles.mesh_parts.basic_parts import create_from_height_map 32 | 33 | 34 | @dataclass 35 | class IndoorPattern(MeshPattern): 36 | dim: Tuple[float, float, float] = (2.0, 2.0, 2.0) # x, y, z 37 | seed: int = 1234 38 | mesh_parts: Tuple[MeshPartsCfg, ...] = ( 39 | tuple(generate_walls(dim)) 40 | + tuple(generate_platforms(name="platform_1", dim=dim, max_h=1.0, min_h=0.0, weight=0.5)) 41 | + tuple(generate_platforms(name="platform_2", dim=dim, max_h=2.0, min_h=0.0, weight=0.5)) 42 | + tuple(generate_platforms(name="platform_2_1", dim=dim, max_h=2.0, min_h=1.0, weight=0.5)) 43 | + tuple(generate_platforms(name="platform_0.5", dim=dim, max_h=0.5, min_h=0.0, weight=0.5)) 44 | + tuple(generate_platforms(name="platform_1_0.5", dim=dim, max_h=1.0, min_h=0.5, weight=0.5)) 45 | + tuple(generate_stepping_stones(name="stepping_1", dim=dim, max_h=1.0, min_h=0.0, weight=1.2)) 46 | + tuple(generate_stepping_stones(name="stepping_2", dim=dim, max_h=2.0, min_h=0.0, weight=1.2)) 47 | + tuple(generate_stepping_stones(name="stepping_2_1", dim=dim, max_h=2.0, min_h=1.0, weight=1.2)) 48 | + tuple(generate_stepping_stones(name="stepping_0.5", dim=dim, max_h=0.5, min_h=0.0, weight=1.2)) 49 | + tuple(generate_stepping_stones(name="stepping_1_0.5", dim=dim, max_h=1.0, min_h=0.5, weight=1.2)) 50 | + tuple(generate_narrow(name="narrow_1", dim=dim, max_h=1.0, min_h=0.0, weight=0.2)) 51 | + tuple(generate_narrow(name="narrow_2", dim=dim, max_h=2.0, min_h=0.0, weight=0.2)) 52 | + tuple(generate_narrow(name="narrow_2_1", dim=dim, max_h=2.0, min_h=1.0, weight=0.2)) 53 | + tuple(generate_narrow(name="narrow_0.5", dim=dim, max_h=0.5, min_h=0.0, weight=0.2)) 54 | + tuple(generate_narrow(name="narrow_1_0.5", dim=dim, max_h=1.0, min_h=0.5, weight=0.2)) 55 | + tuple( 56 | generate_floating_boxes(name="floating_boxes", n=30, dim=dim, seed=seed, array_shape=[5, 5], weight=10.0) 57 | ) 58 | + tuple(generate_stair_parts(name="stair", dim=dim, seed=seed, array_shape=[15, 15], weight=1.0, depth_num=2)) 59 | + tuple( 60 | generate_stair_parts( 61 | name="stair_offset", dim=dim, seed=seed, array_shape=[15, 15], weight=2.0, depth_num=2, offset=1.0 62 | ) 63 | ) 64 | + tuple( 65 | generate_stair_parts( 66 | name="stair_low", dim=dim, total_height=0.5, seed=seed, array_shape=[15, 15], weight=1.0, depth_num=2 67 | ) 68 | ) 69 | + tuple( 70 | generate_stair_parts( 71 | name="stair_low_offset", 72 | dim=dim, 73 | total_height=0.5, 74 | offset=0.5, 75 | seed=seed, 76 | array_shape=[15, 15], 77 | weight=1.0, 78 | depth_num=2, 79 | ) 80 | ) 81 | + tuple( 82 | generate_stair_parts( 83 | name="stair_low_offset_1", 84 | dim=dim, 85 | total_height=0.5, 86 | offset=1.0, 87 | seed=seed, 88 | array_shape=[15, 15], 89 | weight=1.0, 90 | depth_num=2, 91 | ) 92 | ) 93 | + tuple( 94 | generate_stair_parts( 95 | name="stair_low_offset_2", 96 | dim=dim, 97 | total_height=0.5, 98 | offset=1.5, 99 | seed=seed, 100 | array_shape=[15, 15], 101 | weight=1.0, 102 | depth_num=2, 103 | ) 104 | ) 105 | + tuple( 106 | generate_ramp_parts( 107 | name="ramp", 108 | dim=dim, 109 | seed=seed, 110 | array_shape=[30, 30], 111 | total_height=1.0, 112 | offset=0.00, 113 | weight=1.0, 114 | depth_num=1, 115 | ) 116 | ) 117 | + tuple( 118 | generate_ramp_parts( 119 | name="ramp_low", 120 | dim=dim, 121 | seed=seed, 122 | array_shape=[30, 30], 123 | total_height=0.5, 124 | offset=0.00, 125 | weight=1.0, 126 | depth_num=1, 127 | ) 128 | ) 129 | # + tuple(generate_perlin_tile_configs(name="perlin_0", dim=dim, seed=seed, weight=1.2)) 130 | # + tuple(generate_perlin_tile_configs(name="perlin_0.5", dim=dim, seed=seed, weight=1.2, offset=0.5)) 131 | # + tuple(generate_perlin_tile_configs(name="perlin_1", dim=dim, seed=seed, weight=1.2, offset=1.0)) 132 | ) 133 | 134 | 135 | @dataclass 136 | class IndoorPatternLevels(MeshPattern): 137 | dim: Tuple[float, float, float] = (2.0, 2.0, 2.0) # x, y, z 138 | seed: int = 1234 139 | levels: Tuple[float, ...] = (0.0, 0.5, 1.0, 1.5, 2.0) 140 | wall_height: float = 0.5 141 | mesh_parts: Tuple[MeshPartsCfg, ...] = () 142 | 143 | def __post_init__(self): 144 | cfgs = () 145 | dim = self.dim 146 | seed = self.seed 147 | wall_height = self.wall_height 148 | min_hs = self.levels[:-1] + tuple( 149 | [self.levels[0], self.levels[2]] 150 | ) # + tuple([0.0 for _ in range(len(self.levels) - 2)]) 151 | max_hs = self.levels[1:] + tuple([self.levels[2], self.levels[2 + 2]]) # + self.levels[2:] 152 | # for i in range(len(self.levels) - 2): 153 | for min_h, max_h in zip(min_hs, max_hs): 154 | # min_h = self.levels[i] 155 | # max_h = self.levels[i + 1] 156 | cfg = ( 157 | tuple(generate_walls(dim, wall_height=wall_height)) 158 | + tuple( 159 | generate_platforms( 160 | name=f"platform_{min_h}_{max_h}", 161 | dim=dim, 162 | max_h=max_h, 163 | min_h=min_h, 164 | weight=0.5, 165 | wall_height=wall_height, 166 | ) 167 | ) 168 | + tuple( 169 | generate_stepping_stones( 170 | name=f"stepping_{min_h}_{max_h}", dim=dim, max_h=max_h, min_h=min_h, weight=1.2 171 | ) 172 | ) 173 | + tuple(generate_narrow(name=f"narrow_{min_h}_{max_h}", dim=dim, max_h=max_h, min_h=min_h, weight=0.2)) 174 | + tuple( 175 | generate_floating_boxes( 176 | name=f"floating_boxes_{min_h}_{max_h}", 177 | n=30, 178 | dim=dim, 179 | max_h=max_h, 180 | min_h=min_h, 181 | seed=seed, 182 | array_shape=[5, 5], 183 | weight=1.05, 184 | ) 185 | ) 186 | + tuple( 187 | generate_stair_parts( 188 | name=f"stair_{min_h}_{max_h}", 189 | dim=dim, 190 | seed=seed, 191 | array_shape=[15, 15], 192 | weight=1.0, 193 | depth_num=2, 194 | total_height=max_h - min_h, 195 | wall_height=wall_height, 196 | offset=min_h, 197 | ) 198 | ) 199 | + tuple( 200 | generate_ramp_parts( 201 | name=f"ramp_{min_h}_{max_h}", 202 | dim=dim, 203 | seed=seed, 204 | array_shape=[30, 30], 205 | total_height=max_h - min_h, 206 | offset=min_h, 207 | weight=1.0, 208 | depth_num=1, 209 | ) 210 | ) 211 | ) 212 | cfgs += cfg 213 | self.mesh_parts = cfgs 214 | 215 | 216 | if __name__ == "__main__": 217 | from terrain_generator.utils import get_height_array_of_mesh 218 | 219 | cfg = IndoorPatternLevels() 220 | # print(cfg) 221 | keywords = ["floating_boxes_0.0_1.0"] 222 | for mesh_part in cfg.mesh_parts: 223 | print("name ", mesh_part.name) 224 | if any([keyword in mesh_part.name for keyword in keywords]): 225 | print(mesh_part) 226 | tile = create_mesh_tile(mesh_part) 227 | print("tile ", tile) 228 | mesh = tile.get_mesh() 229 | print("mesh ", mesh) 230 | mesh.show() 231 | print(get_height_array_of_mesh(mesh, mesh_part.dim, 5)) 232 | -------------------------------------------------------------------------------- /examples/configs/navigation_cfg.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | from typing import Tuple, Optional 7 | from dataclasses import dataclass 8 | 9 | from numpy.random import f 10 | 11 | from terrain_generator.trimesh_tiles.mesh_parts.mesh_parts_cfg import ( 12 | MeshPattern, 13 | MeshPartsCfg, 14 | WallPartsCfg, 15 | StairMeshPartsCfg, 16 | PlatformMeshPartsCfg, 17 | HeightMapMeshPartsCfg, 18 | ) 19 | from terrain_generator.trimesh_tiles.mesh_parts.rough_parts import generate_perlin_tile_configs 20 | 21 | from terrain_generator.trimesh_tiles.patterns.pattern_generator import ( 22 | generate_walls, 23 | generate_floating_boxes, 24 | generate_narrow, 25 | generate_platforms, 26 | generate_ramp_parts, 27 | generate_stair_parts, 28 | generate_stepping_stones, 29 | ) 30 | from terrain_generator.trimesh_tiles.mesh_parts.create_tiles import create_mesh_tile 31 | from terrain_generator.trimesh_tiles.mesh_parts.basic_parts import create_from_height_map 32 | 33 | 34 | @dataclass 35 | class IndoorNavigationPattern(MeshPattern): 36 | dim: Tuple[float, float, float] = (2.0, 2.0, 2.0) # x, y, z 37 | seed: int = 1234 38 | mesh_parts: Tuple[MeshPartsCfg, ...] = ( 39 | tuple(generate_walls(dim)) 40 | + tuple(generate_platforms(name="platform_1", dim=dim, max_h=1.0, min_h=0.0, weight=0.5)) 41 | + tuple(generate_platforms(name="platform_2", dim=dim, max_h=2.0, min_h=0.0, weight=0.5)) 42 | + tuple(generate_platforms(name="platform_2_1", dim=dim, max_h=2.0, min_h=1.0, weight=0.5)) 43 | + tuple(generate_platforms(name="platform_0.5", dim=dim, max_h=0.5, min_h=0.0, weight=0.5)) 44 | + tuple(generate_platforms(name="platform_1_0.5", dim=dim, max_h=1.0, min_h=0.5, weight=0.5)) 45 | # + tuple(generate_stepping_stones(name="stepping_1", dim=dim, max_h=1.0, min_h=0.0, weight=1.2)) 46 | # + tuple(generate_stepping_stones(name="stepping_2", dim=dim, max_h=2.0, min_h=0.0, weight=1.2)) 47 | # + tuple(generate_stepping_stones(name="stepping_2_1", dim=dim, max_h=2.0, min_h=1.0, weight=1.2)) 48 | # + tuple(generate_stepping_stones(name="stepping_0.5", dim=dim, max_h=0.5, min_h=0.0, weight=1.2)) 49 | # + tuple(generate_stepping_stones(name="stepping_1_0.5", dim=dim, max_h=1.0, min_h=0.5, weight=1.2)) 50 | # + tuple(generate_narrow(name="narrow_1", dim=dim, max_h=1.0, min_h=0.0, weight=0.2)) 51 | # + tuple(generate_narrow(name="narrow_2", dim=dim, max_h=2.0, min_h=0.0, weight=0.2)) 52 | # + tuple(generate_narrow(name="narrow_2_1", dim=dim, max_h=2.0, min_h=1.0, weight=0.2)) 53 | # + tuple(generate_narrow(name="narrow_0.5", dim=dim, max_h=0.5, min_h=0.0, weight=0.2)) 54 | # + tuple(generate_narrow(name="narrow_1_0.5", dim=dim, max_h=1.0, min_h=0.5, weight=0.2)) 55 | + tuple( 56 | generate_floating_boxes(name="floating_boxes", n=30, dim=dim, seed=seed, array_shape=[5, 5], weight=10.0) 57 | ) 58 | + tuple(generate_stair_parts(name="stair", dim=dim, seed=seed, array_shape=[15, 15], weight=1.0, depth_num=2)) 59 | + tuple( 60 | generate_stair_parts( 61 | name="stair_offset", dim=dim, seed=seed, array_shape=[15, 15], weight=2.0, depth_num=2, offset=1.0 62 | ) 63 | ) 64 | + tuple( 65 | generate_stair_parts( 66 | name="stair_low", dim=dim, total_height=0.5, seed=seed, array_shape=[15, 15], weight=1.0, depth_num=2 67 | ) 68 | ) 69 | + tuple( 70 | generate_stair_parts( 71 | name="stair_low_offset", 72 | dim=dim, 73 | total_height=0.5, 74 | offset=0.5, 75 | seed=seed, 76 | array_shape=[15, 15], 77 | weight=1.0, 78 | depth_num=2, 79 | ) 80 | ) 81 | + tuple( 82 | generate_stair_parts( 83 | name="stair_low_offset_1", 84 | dim=dim, 85 | total_height=0.5, 86 | offset=1.0, 87 | seed=seed, 88 | array_shape=[15, 15], 89 | weight=1.0, 90 | depth_num=2, 91 | ) 92 | ) 93 | + tuple( 94 | generate_stair_parts( 95 | name="stair_low_offset_2", 96 | dim=dim, 97 | total_height=0.5, 98 | offset=1.5, 99 | seed=seed, 100 | array_shape=[15, 15], 101 | weight=1.0, 102 | depth_num=2, 103 | ) 104 | ) 105 | + tuple( 106 | generate_ramp_parts( 107 | name="ramp", 108 | dim=dim, 109 | seed=seed, 110 | array_shape=[30, 30], 111 | total_height=1.0, 112 | offset=0.00, 113 | weight=1.0, 114 | depth_num=1, 115 | ) 116 | ) 117 | + tuple( 118 | generate_ramp_parts( 119 | name="ramp_low", 120 | dim=dim, 121 | seed=seed, 122 | array_shape=[30, 30], 123 | total_height=0.5, 124 | offset=0.00, 125 | weight=1.0, 126 | depth_num=1, 127 | ) 128 | ) 129 | # + tuple(generate_perlin_tile_configs(name="perlin_0", dim=dim, seed=seed, weight=1.2)) 130 | # + tuple(generate_perlin_tile_configs(name="perlin_0.5", dim=dim, seed=seed, weight=1.2, offset=0.5)) 131 | # + tuple(generate_perlin_tile_configs(name="perlin_1", dim=dim, seed=seed, weight=1.2, offset=1.0)) 132 | ) 133 | 134 | 135 | @dataclass 136 | class IndoorNavigationPatternLevels(MeshPattern): 137 | dim: Tuple[float, float, float] = (2.0, 2.0, 2.0) # x, y, z 138 | seed: int = 1234 139 | levels: Tuple[float, ...] = (0.0, 0.5, 1.0, 1.5, 2.0) 140 | wall_height: float = 3.0 141 | mesh_parts: Tuple[MeshPartsCfg, ...] = () 142 | 143 | def __post_init__(self): 144 | cfgs = () 145 | dim = self.dim 146 | seed = self.seed 147 | wall_height = self.wall_height 148 | min_hs = ( 149 | self.levels[:-1] 150 | + tuple([self.levels[0], self.levels[2]]) 151 | + tuple([0.0 for _ in range(len(self.levels) - 2)]) 152 | ) 153 | max_hs = self.levels[1:] + tuple([self.levels[2], self.levels[2 + 2]]) + self.levels[2:] 154 | # for i in range(len(self.levels) - 2): 155 | for min_h, max_h in zip(min_hs, max_hs): 156 | # min_h = self.levels[i] 157 | # max_h = self.levels[i + 1] 158 | cfg = ( 159 | tuple(generate_walls(dim, wall_height=wall_height)) 160 | + tuple( 161 | generate_platforms( 162 | name=f"platform_{min_h}_{max_h}", 163 | dim=dim, 164 | max_h=max_h, 165 | min_h=min_h, 166 | weight=0.5, 167 | wall_height=wall_height, 168 | ) 169 | ) 170 | # + tuple( 171 | # generate_stepping_stones( 172 | # name=f"stepping_{min_h}_{max_h}", dim=dim, max_h=max_h, min_h=min_h, weight=1.2 173 | # ) 174 | # ) 175 | # + tuple(generate_narrow(name=f"narrow_{min_h}_{max_h}", dim=dim, max_h=max_h, min_h=min_h, weight=0.2)) 176 | # + tuple( 177 | # generate_floating_boxes( 178 | # name=f"floating_boxes_{min_h}_{max_h}", 179 | # n=30, 180 | # dim=dim, 181 | # max_h=max_h, 182 | # min_h=min_h, 183 | # seed=seed, 184 | # array_shape=[5, 5], 185 | # weight=1.05, 186 | # ) 187 | # ) 188 | + tuple( 189 | generate_stair_parts( 190 | name=f"stair_{min_h}_{max_h}", 191 | dim=dim, 192 | seed=seed, 193 | array_shape=[15, 15], 194 | weight=1.0, 195 | depth_num=2, 196 | total_height=max_h - min_h, 197 | wall_height=wall_height, 198 | offset=min_h, 199 | ) 200 | ) 201 | + tuple( 202 | generate_ramp_parts( 203 | name=f"ramp_{min_h}_{max_h}", 204 | dim=dim, 205 | seed=seed, 206 | array_shape=[30, 30], 207 | total_height=max_h - min_h, 208 | offset=min_h, 209 | weight=1.0, 210 | depth_num=1, 211 | ) 212 | ) 213 | ) 214 | cfgs += cfg 215 | self.mesh_parts = cfgs 216 | 217 | 218 | if __name__ == "__main__": 219 | from terrain_generator.utils import get_height_array_of_mesh 220 | 221 | cfg = IndoorNavigationPatternLevels() 222 | # print(cfg) 223 | keywords = ["floating_boxes_0.0_1.0"] 224 | for mesh_part in cfg.mesh_parts: 225 | print("name ", mesh_part.name) 226 | if any([keyword in mesh_part.name for keyword in keywords]): 227 | print(mesh_part) 228 | tile = create_mesh_tile(mesh_part) 229 | print("tile ", tile) 230 | mesh = tile.get_mesh() 231 | print("mesh ", mesh) 232 | mesh.show() 233 | print(get_height_array_of_mesh(mesh, mesh_part.dim, 5)) 234 | -------------------------------------------------------------------------------- /examples/configs/overhanging_cfg.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | from typing import Tuple, Optional 7 | from dataclasses import dataclass 8 | from itertools import product 9 | 10 | from numpy.random import f 11 | 12 | from terrain_generator.trimesh_tiles.mesh_parts.mesh_parts_cfg import ( 13 | OverhangingMeshPartsCfg, 14 | OverhangingBoxesPartsCfg, 15 | MeshPattern, 16 | MeshPartsCfg, 17 | WallPartsCfg, 18 | StairMeshPartsCfg, 19 | PlatformMeshPartsCfg, 20 | HeightMapMeshPartsCfg, 21 | FloatingBoxesPartsCfg, 22 | ) 23 | from terrain_generator.trimesh_tiles.mesh_parts.rough_parts import generate_perlin_tile_configs 24 | 25 | from terrain_generator.trimesh_tiles.patterns.pattern_generator import ( 26 | generate_random_box_platform, 27 | # generate_walls, 28 | generate_floating_boxes, 29 | generate_narrow, 30 | generate_platforms, 31 | generate_ramp_parts, 32 | generate_stair_parts, 33 | generate_stepping_stones, 34 | generate_floating_capsules, 35 | generate_random_boxes, 36 | generate_overhanging_platforms, 37 | add_capsules, 38 | ) 39 | from terrain_generator.trimesh_tiles.patterns.overhanging_patterns import generate_walls 40 | from terrain_generator.trimesh_tiles.mesh_parts.create_tiles import create_mesh_tile 41 | from terrain_generator.trimesh_tiles.mesh_parts.basic_parts import create_from_height_map 42 | 43 | 44 | @dataclass 45 | class OverhangingTerrainPattern(MeshPattern): 46 | dim: Tuple[float, float, float] = (2.0, 2.0, 2.0) # x, y, z 47 | seed: int = 1234 48 | 49 | enable_wall: bool = False 50 | 51 | # random box platform 52 | random_cfgs = [] 53 | n_random_boxes: int = 10 54 | random_box_weight: float = 0.01 55 | perlin_weight: float = 0.1 56 | for i in range(n_random_boxes): 57 | random_cfgs += generate_random_box_platform( 58 | name=f"box_platform_flat_{i}", 59 | offset=0.0, 60 | height_diff=0.0, 61 | height_std=0.1, 62 | n=6, 63 | dim=dim, 64 | weight=random_box_weight / n_random_boxes, 65 | ) 66 | random_cfgs += generate_random_box_platform( 67 | name=f"box_platform_flat_1.0{i}", 68 | offset=1.0, 69 | height_diff=0.0, 70 | height_std=0.1, 71 | n=6, 72 | dim=dim, 73 | weight=random_box_weight / n_random_boxes, 74 | ) 75 | random_cfgs += generate_random_box_platform( 76 | name=f"box_platform_flat_2.0{i}", 77 | offset=2.0, 78 | height_diff=0.0, 79 | height_std=0.1, 80 | n=6, 81 | dim=dim, 82 | weight=random_box_weight / n_random_boxes, 83 | ) 84 | random_cfgs += generate_random_box_platform( 85 | name=f"box_platform_flat_8_{i}", 86 | offset=0.0, 87 | height_diff=0.0, 88 | height_std=0.1, 89 | n=8, 90 | dim=dim, 91 | weight=random_box_weight / n_random_boxes, 92 | ) 93 | # random_cfgs += generate_random_box_platform( 94 | # name=f"box_platform_flat_large_{i}", 95 | # offset=0.0, 96 | # height_diff=0.0, 97 | # height_std=0.5, 98 | # n=8, 99 | # dim=dim, 100 | # weight=random_box_weight / n_random_boxes, 101 | # ) 102 | # random_cfgs += generate_random_box_platform( 103 | # name=f"box_platform_0.0{i}", 104 | # offset=0.0, 105 | # height_diff=0.5, 106 | # height_std=0.1, 107 | # n=6, 108 | # dim=dim, 109 | # weight=random_box_weight / n_random_boxes, 110 | # ) 111 | # random_cfgs += generate_random_box_platform( 112 | # name=f"box_platform_0.5{i}", 113 | # offset=0.5, 114 | # height_diff=0.5, 115 | # height_std=0.1, 116 | # n=6, 117 | # dim=dim, 118 | # weight=random_box_weight / n_random_boxes, 119 | # ) 120 | # random_cfgs += generate_random_box_platform( 121 | # name=f"box_platform_1.0_{i}", 122 | # offset=1.0, 123 | # height_diff=0.5, 124 | # height_std=0.1, 125 | # n=6, 126 | # dim=dim, 127 | # weight=random_box_weight / n_random_boxes, 128 | # ) 129 | # random_cfgs += generate_random_box_platform( 130 | # name=f"box_platform_1.5_{i}", 131 | # offset=1.5, 132 | # height_diff=0.5, 133 | # height_std=0.1, 134 | # n=6, 135 | # dim=dim, 136 | # weight=random_box_weight / n_random_boxes, 137 | # ) 138 | random_cfgs += generate_random_box_platform( 139 | name=f"box_platform_diff_1.0_{i}", 140 | offset=0.0, 141 | height_diff=1.0, 142 | height_std=0.1, 143 | n=6, 144 | dim=dim, 145 | weight=random_box_weight / n_random_boxes, 146 | ) 147 | random_cfgs += generate_random_box_platform( 148 | name=f"box_platform_diff_1.0_std{i}", 149 | offset=0.0, 150 | height_diff=1.0, 151 | height_std=0.15, 152 | n=6, 153 | dim=dim, 154 | weight=random_box_weight / n_random_boxes, 155 | ) 156 | random_cfgs += generate_random_box_platform( 157 | name=f"box_platform_diff_2.0_{i}", 158 | offset=1.0, 159 | height_diff=1.0, 160 | height_std=0.1, 161 | n=6, 162 | dim=dim, 163 | weight=random_box_weight / n_random_boxes, 164 | ) 165 | random_cfgs += generate_perlin_tile_configs(f"perlin_{i}", [2, 2, 2], weight=perlin_weight / n_random_boxes) 166 | random_cfgs += generate_perlin_tile_configs( 167 | f"perlin_0.5_{i}", [2, 2, 2], weight=perlin_weight / n_random_boxes, offset=0.5, height=0.5 168 | ) 169 | random_cfgs += generate_perlin_tile_configs( 170 | f"perlin_1.0_{i}", [2, 2, 2], weight=perlin_weight / n_random_boxes, offset=0.0, height=1.0 171 | ) 172 | random_cfgs += generate_perlin_tile_configs( 173 | f"perlin_1.0_1.0{i}", [2, 2, 2], weight=perlin_weight / n_random_boxes, offset=1.0, height=1.0 174 | ) 175 | mesh_parts: Tuple[MeshPartsCfg, ...] = ( 176 | (WallPartsCfg(name=f"floor", dim=dim, wall_edges=(), weight=0.01),) 177 | + tuple( 178 | generate_platforms(name="platform_1", dim=dim, max_h=1.0, min_h=0.0, weight=0.5, enable_wall=enable_wall) 179 | ) 180 | + tuple( 181 | generate_platforms(name="platform_2", dim=dim, max_h=2.0, min_h=0.0, weight=0.5, enable_wall=enable_wall) 182 | ) 183 | + tuple( 184 | generate_platforms(name="platform_2_1", dim=dim, max_h=2.0, min_h=1.0, weight=0.5, enable_wall=enable_wall) 185 | ) 186 | # + tuple( 187 | # generate_platforms(name="platform_0.5", dim=dim, max_h=0.5, min_h=0.0, weight=0.5, enable_wall=enable_wall) 188 | # ) 189 | # + tuple( 190 | # generate_platforms( 191 | # name="platform_1_0.5", dim=dim, max_h=1.0, min_h=0.5, weight=0.5, enable_wall=enable_wall 192 | # ) 193 | # ) 194 | + tuple(random_cfgs) 195 | + tuple( 196 | generate_stair_parts( 197 | name="stair", dim=dim, seed=seed, array_shape=[15, 15], weight=0.5, depth_num=2, enable_wall=enable_wall 198 | ) 199 | ) 200 | + tuple( 201 | generate_stair_parts( 202 | name="stair_offset", 203 | dim=dim, 204 | seed=seed, 205 | array_shape=[15, 15], 206 | weight=2.0, 207 | depth_num=2, 208 | offset=1.0, 209 | enable_wall=enable_wall, 210 | ) 211 | ) 212 | + tuple( 213 | generate_stair_parts( 214 | name="stair_low", 215 | dim=dim, 216 | total_height=0.5, 217 | seed=seed, 218 | array_shape=[15, 15], 219 | weight=0.5, 220 | depth_num=2, 221 | enable_wall=enable_wall, 222 | ) 223 | ) 224 | + tuple( 225 | generate_stair_parts( 226 | name="stair_low_offset", 227 | dim=dim, 228 | total_height=0.5, 229 | offset=0.5, 230 | seed=seed, 231 | array_shape=[15, 15], 232 | weight=0.5, 233 | depth_num=2, 234 | enable_wall=enable_wall, 235 | ) 236 | ) 237 | + tuple( 238 | generate_stair_parts( 239 | name="stair_low_offset_1", 240 | dim=dim, 241 | total_height=0.5, 242 | offset=1.0, 243 | seed=seed, 244 | array_shape=[15, 15], 245 | weight=0.5, 246 | depth_num=2, 247 | enable_wall=enable_wall, 248 | ) 249 | ) 250 | + tuple( 251 | generate_stair_parts( 252 | name="stair_low_offset_2", 253 | dim=dim, 254 | total_height=0.5, 255 | offset=1.5, 256 | seed=seed, 257 | array_shape=[15, 15], 258 | weight=0.5, 259 | depth_num=2, 260 | enable_wall=enable_wall, 261 | ) 262 | ) 263 | + tuple( 264 | generate_ramp_parts( 265 | name="ramp", 266 | dim=dim, 267 | seed=seed, 268 | array_shape=[30, 30], 269 | total_height=1.0, 270 | offset=0.00, 271 | weight=0.05, 272 | depth_num=1, 273 | ) 274 | ) 275 | + tuple( 276 | generate_ramp_parts( 277 | name="ramp_low", 278 | dim=dim, 279 | seed=seed, 280 | array_shape=[30, 30], 281 | total_height=0.5, 282 | offset=0.00, 283 | weight=0.2, 284 | depth_num=1, 285 | ) 286 | ) 287 | ) 288 | 289 | 290 | @dataclass 291 | class OverhangingFloorPattern(MeshPattern): 292 | dim: Tuple[float, float, float] = (2.0, 2.0, 2.0) # x, y, z 293 | seed: int = 1234 294 | 295 | enable_wall: bool = False 296 | random_cfgs = [] 297 | n_random_boxes: int = 10 298 | random_box_weight: float = 0.01 299 | perlin_weight: float = 0.1 300 | for i in range(n_random_boxes): 301 | random_cfgs += generate_random_box_platform( 302 | name=f"box_platform_flat_{i}", 303 | offset=0.0, 304 | height_diff=0.0, 305 | height_std=0.1, 306 | n=6, 307 | dim=dim, 308 | weight=random_box_weight / n_random_boxes, 309 | ) 310 | random_cfgs += generate_random_box_platform( 311 | name=f"box_platform_flat_0.2{i}", 312 | offset=0.2, 313 | height_diff=0.0, 314 | height_std=0.1, 315 | n=6, 316 | dim=dim, 317 | weight=random_box_weight / n_random_boxes, 318 | ) 319 | random_cfgs += generate_random_box_platform( 320 | name=f"box_platform_flat_0.3{i}", 321 | offset=0.3, 322 | height_diff=0.0, 323 | height_std=0.1, 324 | n=6, 325 | dim=dim, 326 | weight=random_box_weight / n_random_boxes, 327 | ) 328 | mesh_parts: Tuple[MeshPartsCfg, ...] = ( 329 | (WallPartsCfg(name=f"floor", dim=dim, wall_edges=(), weight=0.01),) 330 | + tuple( 331 | generate_platforms(name="platform_1", dim=dim, max_h=0.2, min_h=0.0, weight=0.5, enable_wall=enable_wall) 332 | ) 333 | + tuple( 334 | generate_platforms(name="platform_2", dim=dim, max_h=0.3, min_h=0.0, weight=0.5, enable_wall=enable_wall) 335 | ) 336 | + tuple( 337 | generate_platforms(name="platform_2_1", dim=dim, max_h=0.3, min_h=0.2, weight=0.5, enable_wall=enable_wall) 338 | ) 339 | + tuple(random_cfgs) 340 | ) 341 | 342 | 343 | @dataclass 344 | class OverhangingPattern(MeshPattern): 345 | dim: Tuple[float, float, float] = (2.0, 2.0, 2.0) # x, y, z 346 | mesh_parts: Tuple[MeshPartsCfg, ...] = generate_walls(name="walls", dim=dim, wall_height=3.0, wall_thickness=0.4) 347 | overhanging_prob: float = 0.5 348 | gap_means = [0.6, 0.8, 1.0, 1.2] 349 | gap_std = [0.05, 0.1, 0.1, 0.2] 350 | box_height = [0.1, 0.5, 0.5, 0.5, 1.0, 1.0, 2.0, 2.0] 351 | box_grid_n = [3, 4, 6] 352 | overhanging_cfgs = [ 353 | OverhangingBoxesPartsCfg(gap_mean=gap_mean, gap_std=gap_std, box_height=box_height, box_grid_n=box_grid_n) 354 | for gap_mean, gap_std, box_height, box_grid_n in product(gap_means, gap_std, box_height, box_grid_n) 355 | ] 356 | box_dim_mins = [[0.5, 0.5, 0.1], [1.5, 0.05, 0.05]] 357 | box_dim_maxs = [[1.0, 1.0, 0.5], [1.5, 0.1, 0.1]] 358 | min_heights = [0.5, 0.6, 0.7, 1.0] 359 | max_heights = [1.0, 1.0, 1.0, 2.0] 360 | roll_pitch_range = [[0.0, np.pi / 6], [0.0, np.pi / 4], [0.0, np.pi / 2]] 361 | n_boxes = [3, 5] 362 | floating_box_cfgs = [ 363 | FloatingBoxesPartsCfg( 364 | name="floating_boxes", 365 | n_boxes=n_box, 366 | box_dim_min=tuple(box_dim_min), 367 | box_dim_max=tuple(box_dim_max), 368 | roll_pitch_range=(0.0, np.pi / 6), # in rad 369 | min_height=min_height, 370 | ) 371 | for n_box, box_dim_min, box_dim_max, min_height in product(n_boxes, box_dim_mins, box_dim_maxs, min_heights) 372 | ] 373 | # overhanging_cfg_list: Tuple[OverhangingMeshPartsCfg, ...] = tuple(overhanging_cfgs) + tuple(floating_box_cfgs) 374 | overhanging_cfg_list: Tuple[OverhangingMeshPartsCfg, ...] = tuple(floating_box_cfgs) 375 | 376 | 377 | if __name__ == "__main__": 378 | from terrain_generator.utils import get_height_array_of_mesh 379 | 380 | cfg = OverhangingPattern() 381 | print(cfg) 382 | # print(len(cfg.overhanging_cfg_list)) 383 | 384 | # exit(0) 385 | 386 | cfg = OverhangingTerrainPattern() 387 | # print(cfg) 388 | keywords = ["mesh"] 389 | for mesh_part in cfg.mesh_parts: 390 | print("name ", mesh_part.name) 391 | if any([keyword in mesh_part.name for keyword in keywords]): 392 | print(mesh_part) 393 | tile = create_mesh_tile(mesh_part) 394 | print("tile ", tile) 395 | mesh = tile.get_mesh() 396 | print("mesh ", mesh) 397 | mesh.show() 398 | print(get_height_array_of_mesh(mesh, mesh_part.dim, 5)) 399 | -------------------------------------------------------------------------------- /examples/create_primitive_course.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import os 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | import trimesh 9 | import inspect 10 | 11 | from terrain_generator.wfc.wfc import WFCSolver 12 | 13 | from terrain_generator.trimesh_tiles.mesh_parts.create_tiles import create_mesh_pattern 14 | from terrain_generator.utils import visualize_mesh 15 | from terrain_generator.trimesh_tiles.mesh_parts.mesh_parts_cfg import MeshPartsCfg, MeshPattern 16 | 17 | from configs.indoor_cfg import IndoorPattern, IndoorPatternLevels 18 | from configs.navigation_cfg import IndoorNavigationPatternLevels 19 | from alive_progress import alive_bar 20 | 21 | from terrain_generator.trimesh_tiles.primitive_course.steps import * 22 | 23 | 24 | def generate_tiles( 25 | cfg, 26 | mesh_name="result_mesh.stl", 27 | mesh_dir="results/result", 28 | visualize=False, 29 | ): 30 | 31 | dim = cfg.dim 32 | tiles = create_mesh_pattern(cfg) 33 | 34 | result_mesh = trimesh.Trimesh() 35 | floating_mesh = trimesh.Trimesh() 36 | for name, tile in tiles.items(): 37 | if name == "start": 38 | mesh = tile.get_mesh().copy() 39 | xy_offset = np.array([0, 0.0, 0.0]) 40 | mesh.apply_translation(xy_offset) 41 | result_mesh += mesh 42 | elif name == "goal": 43 | mesh = tile.get_mesh().copy() 44 | xy_offset = np.array([0.0, -2 * dim[1], 0.0]) 45 | mesh.apply_translation(xy_offset) 46 | result_mesh += mesh 47 | elif "floating" in name: 48 | mesh = tile.get_mesh().copy() 49 | xy_offset = np.array([0.0, -1 * dim[1], 0.0]) 50 | mesh.apply_translation(xy_offset) 51 | floating_mesh += mesh 52 | else: 53 | mesh = tile.get_mesh().copy() 54 | xy_offset = np.array([0.0, -1 * dim[1], 0.0]) 55 | mesh.apply_translation(xy_offset) 56 | result_mesh += mesh 57 | bbox = result_mesh.bounding_box.bounds 58 | # Get the center of the bounding box. 59 | center = np.mean(bbox, axis=0) 60 | center[2] = 0.0 61 | # Get the size of the bounding box. 62 | result_mesh = result_mesh.apply_translation(-center) 63 | floating_mesh = floating_mesh.apply_translation(-center) 64 | 65 | os.makedirs(mesh_dir, exist_ok=True) 66 | print("saving mesh to ", mesh_name) 67 | result_mesh.export(os.path.join(mesh_dir, mesh_name)) 68 | if floating_mesh.vertices.shape[0] > 0: 69 | # get name before extension 70 | name, ext = os.path.splitext(mesh_name) 71 | floating_mesh.export(os.path.join(mesh_dir, name + "_floating" + ext)) 72 | if visualize: 73 | visualize_mesh(result_mesh) 74 | 75 | 76 | def generate_steps(dim, level, mesh_dir): 77 | height_diff = level * 1.0 78 | cfgs = create_step(MeshPartsCfg(dim=dim), height_diff=height_diff) 79 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 80 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 81 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 82 | 83 | 84 | def generate_gaps(dim, level, mesh_dir): 85 | gap_length = level * 0.8 86 | cfgs = create_gaps(MeshPartsCfg(dim=dim), gap_length=gap_length) 87 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 88 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 89 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 90 | 91 | 92 | def generate_gaps_with_h(dim, level, mesh_dir): 93 | gap_length = level * 0.8 94 | height_diff = level * 0.2 95 | cfgs = create_gaps(MeshPartsCfg(dim=dim), gap_length=gap_length, height_diff=height_diff) 96 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 97 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 98 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 99 | 100 | 101 | def generate_middle_steps(dim, level, mesh_dir): 102 | height_diff = level * 0.5 103 | n = 11 104 | cfgs = create_middle_step(MeshPartsCfg(dim=dim), height_diff=height_diff) 105 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 106 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 107 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 108 | 109 | 110 | def generate_middle_steps_wide(dim, level, mesh_dir): 111 | height_diff = level * 0.5 112 | n = 5 113 | cfgs = create_middle_step(MeshPartsCfg(dim=dim), height_diff=height_diff, n=n) 114 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 115 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 116 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 117 | 118 | 119 | def generate_narrows(dim, level, mesh_dir): 120 | width = (1.0 - level) * 0.5 + 0.1 121 | side_std = 0.0 122 | height_std = 0.0 123 | cfgs = create_narrow(MeshPartsCfg(dim=dim), width=width, side_std=side_std, height_std=height_std) 124 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 125 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 126 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 127 | 128 | 129 | def generate_narrows_with_side(dim, level, mesh_dir): 130 | width = (1.0 - level) * 0.5 + 0.1 131 | side_std = 0.1 * level 132 | height_std = 0.0 133 | cfgs = create_narrow(MeshPartsCfg(dim=dim), width=width, side_std=side_std, height_std=height_std) 134 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 135 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 136 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 137 | 138 | 139 | def generate_narrows_with_side_height(dim, level, mesh_dir): 140 | width = (1.0 - level) * 0.5 + 0.1 141 | side_std = 0.1 * level 142 | height_std = 0.05 * level 143 | cfgs = create_narrow(MeshPartsCfg(dim=dim), width=width, side_std=side_std, height_std=height_std) 144 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 145 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 146 | 147 | 148 | def generate_stepping(dim, level, mesh_dir): 149 | # width = (1.0 - level) * 0.5 + 0.1 150 | width = 0.5 151 | side_std = 0.2 * level 152 | height_std = 0.05 * level 153 | n = 6 154 | ratio = 0.5 + (1.0 - level) * 0.3 155 | cfgs = create_stepping( 156 | MeshPartsCfg(dim=dim), width=width, side_std=side_std, height_std=height_std, n=n, ratio=ratio 157 | ) 158 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 159 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 160 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 161 | 162 | 163 | def generate_box_grid(dim, level, mesh_dir): 164 | height_diff = 0.0 165 | height_std = level * 0.5 166 | n = 8 167 | cfgs = create_box_grid(MeshPartsCfg(dim=dim), height_diff=height_diff, height_std=height_std, n=n) 168 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 169 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 170 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 171 | 172 | 173 | def generate_box_grid_slope(dim, level, mesh_dir): 174 | height_diff = level * 1.5 175 | height_std = level * 0.5 176 | n = 8 177 | cfgs = create_box_grid(MeshPartsCfg(dim=dim), height_diff=height_diff, height_std=height_std, n=n) 178 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 179 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 180 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 181 | 182 | 183 | def generate_box_grid_small(dim, level, mesh_dir): 184 | height_diff = level * 1.0 185 | height_std = level * 0.5 186 | n = 14 187 | cfgs = create_box_grid(MeshPartsCfg(dim=dim), height_diff=height_diff, height_std=height_std, n=n) 188 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 189 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 190 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 191 | 192 | 193 | def generate_floating_box_grid(dim, level, mesh_dir): 194 | height_diff = 0.0 195 | height_gap = (1.0 - level) * 1.0 + 0.7 196 | height_std = 0.2 * level 197 | cfgs = create_floating_box_grid( 198 | MeshPartsCfg(dim=dim), 199 | height_diff=height_diff, 200 | height_std=height_std, 201 | height_gap_mean=height_gap, 202 | height_gap_std=0.1, 203 | n=4, 204 | ) 205 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 206 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 207 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 208 | 209 | 210 | def generate_floating_box_grid_slope(dim, level, mesh_dir): 211 | height_diff = level * 1.0 212 | height_gap = (1.0 - level) * 1.0 + 0.8 213 | cfgs = create_floating_box_grid( 214 | MeshPartsCfg(dim=dim), 215 | height_diff=height_diff, 216 | height_std=0.1, 217 | height_gap_mean=height_gap, 218 | height_gap_std=0.1, 219 | n=10, 220 | ) 221 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 222 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 223 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 224 | 225 | 226 | def generate_random_tunnel(dim, level, mesh_dir): 227 | height_diff = 0.0 228 | height_gap = (1.0 - level) * 1.0 + 0.7 229 | cfgs = create_random_tunnel( 230 | MeshPartsCfg(dim=dim), 231 | height_diff=height_diff, 232 | height_std=0.1, 233 | height_gap_mean=height_gap, 234 | height_gap_std=0.1, 235 | n=8, 236 | wall_n=2, 237 | ) 238 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 239 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 240 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 241 | 242 | 243 | def generate_random_tunnel_narrow(dim, level, mesh_dir): 244 | height_diff = 0.0 245 | height_gap = (1.0 - level) * 1.0 + 0.7 246 | cfgs = create_random_tunnel( 247 | MeshPartsCfg(dim=dim), 248 | height_diff=height_diff, 249 | height_std=0.1, 250 | height_gap_mean=height_gap, 251 | height_gap_std=0.1, 252 | n=8, 253 | wall_n=2, 254 | ) 255 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 256 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 257 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 258 | 259 | 260 | def generate_random_tunnel_slope(dim, level, mesh_dir): 261 | height_diff = 1.0 * level 262 | height_gap = (1.0 - level) * 1.0 + 0.8 263 | cfgs = create_random_tunnel( 264 | MeshPartsCfg(dim=dim), 265 | height_diff=height_diff, 266 | height_std=0.1, 267 | height_gap_mean=height_gap, 268 | height_gap_std=0.1, 269 | n=10, 270 | wall_n=2, 271 | ) 272 | cfg = MeshPattern(dim=dim, mesh_parts=cfgs) 273 | mesh_dir = os.path.join(mesh_dir, inspect.currentframe().f_code.co_name) 274 | generate_tiles(cfg, mesh_name=f"mesh_{level:.1f}.obj", mesh_dir=mesh_dir) 275 | 276 | 277 | if __name__ == "__main__": 278 | 279 | dim = (3.0, 3.0, 3.0) 280 | level = 0.5 281 | mesh_dir = "results/primitive_separated" 282 | 283 | for level in np.arange(0.0, 1.1, 0.1): 284 | # generate_steps(dim, level, mesh_dir) 285 | # generate_gaps(dim, level, mesh_dir) 286 | # generate_gaps_with_h(dim, level, mesh_dir) 287 | # generate_middle_steps(dim, level, mesh_dir) 288 | # generate_middle_steps_wide(dim, level, mesh_dir) 289 | # generate_narrows(dim, level, mesh_dir) 290 | # generate_narrows_with_side(dim, level, mesh_dir) 291 | # generate_narrows_with_side_height(dim, level, mesh_dir) 292 | # generate_stepping(dim, level, mesh_dir) 293 | # generate_box_grid(dim, level, mesh_dir) 294 | # generate_box_grid_slope(dim, level, mesh_dir) 295 | # generate_box_grid_small(dim, level, mesh_dir) 296 | generate_floating_box_grid(dim, level, mesh_dir) 297 | generate_floating_box_grid_slope(dim, level, mesh_dir) 298 | generate_random_tunnel(dim, level, mesh_dir) 299 | generate_random_tunnel_narrow(dim, level, mesh_dir) 300 | generate_random_tunnel_slope(dim, level, mesh_dir) 301 | -------------------------------------------------------------------------------- /examples/generate_mountain.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import os 6 | import trimesh 7 | import numpy as np 8 | from typing import Tuple 9 | 10 | from terrain_generator.trimesh_tiles.mesh_parts.mountain import generate_perlin_terrain 11 | from terrain_generator.trimesh_tiles.mesh_parts.tree import add_trees_on_terrain 12 | 13 | 14 | def generate_mountain(mesh_dir): 15 | 16 | terrain = generate_perlin_terrain(horizontal_scale=0.2, vertical_scale=3.0) 17 | # terrain.show() 18 | tree_mesh = add_trees_on_terrain( 19 | terrain, num_trees=100, tree_scale_range=(0.15, 0.55), tree_deg_range=(-60, 60), tree_cylinder_sections=4 20 | ) 21 | mesh = terrain + tree_mesh 22 | # mesh.show() 23 | bbox = mesh.bounding_box.bounds 24 | # Get the center of the bounding box. 25 | center = np.mean(bbox, axis=0) 26 | center[2] = 0.0 27 | # Translate the mesh to the center of the bounding box. 28 | 29 | mesh = mesh.apply_translation(-center) 30 | 31 | os.makedirs(mesh_dir, exist_ok=True) 32 | mesh.export(os.path.join(mesh_dir, "mountain.obj")) 33 | terrain.export(os.path.join(mesh_dir, "terrain.obj")) 34 | tree_mesh.export(os.path.join(mesh_dir, "tree.obj")) 35 | 36 | 37 | if __name__ == "__main__": 38 | mesh_dir = "results/mountains" 39 | generate_mountain(mesh_dir) 40 | -------------------------------------------------------------------------------- /examples/generate_with_wfc.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import os 6 | import argparse 7 | import numpy as np 8 | import trimesh 9 | from typing import Optional 10 | import random 11 | 12 | from terrain_generator.wfc.wfc import WFCSolver 13 | 14 | from terrain_generator.trimesh_tiles.mesh_parts.create_tiles import create_mesh_pattern, get_mesh_gen 15 | 16 | # from trimesh_tiles.mesh_parts.overhanging_parts import FloorOverhangingParts 17 | from terrain_generator.utils.mesh_utils import visualize_mesh, compute_sdf 18 | from terrain_generator.utils import calc_spawnable_locations_with_sdf 19 | from terrain_generator.trimesh_tiles.mesh_parts.mesh_parts_cfg import ( 20 | MeshPattern, 21 | OverhangingMeshPartsCfg, 22 | OverhangingBoxesPartsCfg, 23 | ) 24 | from terrain_generator.trimesh_tiles.mesh_parts.overhanging_parts import get_cfg_gen 25 | 26 | from configs.navigation_cfg import IndoorNavigationPatternLevels 27 | from configs.overhanging_cfg import OverhangingTerrainPattern, OverhangingPattern, OverhangingFloorPattern 28 | from terrain_generator.navigation.mesh_terrain import MeshTerrain, MeshTerrainCfg 29 | from alive_progress import alive_bar 30 | 31 | 32 | def solve_with_wfc(cfg: MeshPattern, shape, initial_tile_name): 33 | # Create tiles from config 34 | print("Creating tiles...") 35 | tiles = create_mesh_pattern(cfg) 36 | 37 | # Create solver 38 | wfc_solver = WFCSolver(shape=shape, dimensions=2, seed=None) 39 | 40 | # Add tiles to the solver 41 | for tile in tiles.values(): 42 | wfc_solver.register_tile(*tile.get_dict_tile()) 43 | # print(f"Tile: {tile.name}, {tile.array}") 44 | 45 | # Place initial tile in the center. 46 | init_tiles = [(initial_tile_name, (wfc_solver.shape[0] // 2, wfc_solver.shape[1] // 2))] 47 | wave = wfc_solver.run(init_tiles=init_tiles, max_steps=10000) 48 | wave_order = wfc_solver.wfc.wave.wave_order 49 | names = wfc_solver.names 50 | return tiles, wave, wave_order, names 51 | 52 | 53 | def create_mesh_from_cfg( 54 | cfg: MeshPattern, 55 | overhanging_cfg: Optional[OverhangingPattern] = None, 56 | prefix="mesh", 57 | mesh_dir="results/result", 58 | shape=[20, 20], 59 | initial_tile_name="floor", 60 | overhanging_initial_tile_name="walls_empty", 61 | visualize=False, 62 | enable_history=False, 63 | enable_sdf=False, 64 | sdf_resolution=0.1, 65 | ): 66 | """Generate terrain mesh from config. 67 | It will generate a mesh from the given config and save it to the given path. 68 | Args: 69 | cfg: MeshPattern config 70 | mesh_name: name of the mesh file 71 | mesh_dir: directory to save the mesh file 72 | shape: shape of the whole tile. 73 | initial_tile_name: name of the initial tile which is positioned at the center of the tile. 74 | visualize: visualize the mesh or not 75 | enable_history: save the history of the solver or not 76 | """ 77 | os.makedirs(mesh_dir, exist_ok=True) 78 | save_dir = os.path.join(mesh_dir, prefix) 79 | os.makedirs(save_dir, exist_ok=True) 80 | 81 | tiles, wave, wave_order, wave_names = solve_with_wfc(cfg, shape, initial_tile_name) 82 | 83 | if overhanging_cfg is not None: 84 | over_tiles, over_wave, over_wave_order, over_wave_names = solve_with_wfc( 85 | overhanging_cfg, shape, overhanging_initial_tile_name 86 | ) 87 | 88 | # Save the history of the solver 89 | if enable_history: 90 | history_dir = os.path.join(mesh_dir, f"{prefix}_history") 91 | os.makedirs(history_dir, exist_ok=True) 92 | np.save(os.path.join(history_dir, "wave.npy"), wave) 93 | np.save(os.path.join(history_dir, "wave_order.npy"), wave_order) 94 | 95 | # We can visualize the wave propagation. We save the mesh for each step. 96 | parts_dir = os.path.join(history_dir, "parts") 97 | os.makedirs(parts_dir, exist_ok=True) 98 | translated_parts_dir = os.path.join(history_dir, "translated_parts") 99 | os.makedirs(translated_parts_dir, exist_ok=True) 100 | 101 | print("Converting to mesh...") 102 | # Compose the whole mesh from the tiles 103 | result_mesh = trimesh.Trimesh() 104 | if overhanging_cfg is not None: 105 | result_terrain_mesh = trimesh.Trimesh() 106 | result_overhanging_mesh = trimesh.Trimesh() 107 | 108 | if enable_sdf: 109 | sdf_dim = np.array(cfg.dim) * 3 # to merge with neighboring tiles 110 | sdf_dim[2] = 3.0 * 2 111 | sdf_array_dim = (np.array(wave.shape) + 2) * cfg.dim[:2] / sdf_resolution 112 | sdf_array_dim = np.array([sdf_array_dim[0], sdf_array_dim[1], sdf_dim[2] / sdf_resolution], dtype=int) 113 | sdf_min = np.inf * np.ones(sdf_array_dim, dtype=np.float32) 114 | 115 | with alive_bar(len(wave.flatten())) as bar: 116 | for y in range(wave.shape[0]): 117 | for x in range(wave.shape[1]): 118 | terrain_mesh = tiles[wave_names[wave[y, x]]].get_mesh().copy() 119 | mesh = terrain_mesh.copy() 120 | 121 | if overhanging_cfg is not None: 122 | over_mesh = trimesh.Trimesh() 123 | # terrain_mesh += mesh 124 | if np.random.rand() < overhanging_cfg.overhanging_prob: 125 | mesh_cfg = random.choice(overhanging_cfg.overhanging_cfg_list) 126 | mesh_cfg.mesh = mesh 127 | # over_box_mesh_cfg = create_overhanging_boxes(mesh_cfg) 128 | cfg_gen = get_cfg_gen(mesh_cfg) 129 | over_mesh_cfg = cfg_gen(mesh_cfg) 130 | over_box_mesh = get_mesh_gen(over_mesh_cfg)(over_mesh_cfg) 131 | over_mesh += over_box_mesh 132 | # over_mesh += over_tiles[over_wave_names[over_wave[y, x]]].get_mesh().copy() 133 | mesh += over_mesh 134 | if enable_sdf: 135 | # Compute SDF around the mesh 136 | mesh_sdf = compute_sdf(mesh, dim=sdf_dim, resolution=0.1) 137 | x_min = int(x * cfg.dim[0] / sdf_resolution) 138 | y_min = int((wave.shape[0] - y - 1) * cfg.dim[1] / sdf_resolution) 139 | x_max = int((x + 2 + 1) * cfg.dim[0] / sdf_resolution) 140 | y_max = int((wave.shape[0] - y + 2) * cfg.dim[1] / sdf_resolution) 141 | # Update sdf_min by comparing the relevant part 142 | sdf_min[x_min:x_max, y_min:y_max, :] = np.minimum(sdf_min[x_min:x_max, y_min:y_max, :], mesh_sdf) 143 | 144 | # save original parts for visualization 145 | if enable_history: 146 | mesh.export(os.path.join(parts_dir, f"{wave[y, x]}_{y}_{x}_{wave_names[wave[y, x]]}.obj")) 147 | if overhanging_cfg is not None: 148 | over_mesh.export( 149 | os.path.join(parts_dir, f"{over_wave[y, x]}_{y}_{x}_{over_wave_names[over_wave[y, x]]}.obj") 150 | ) 151 | terrain_mesh.export( 152 | os.path.join(parts_dir, f"{wave[y, x]}_{y}_{x}_{wave_names[wave[y, x]]}_terrain.obj") 153 | ) 154 | # Translate to the position of the tile 155 | xy_offset = np.array([x * cfg.dim[0], -y * cfg.dim[1], 0.0]) 156 | mesh.apply_translation(xy_offset) 157 | if overhanging_cfg is not None: 158 | over_mesh.apply_translation(xy_offset) 159 | terrain_mesh.apply_translation(xy_offset) 160 | if enable_history: 161 | mesh.export( 162 | os.path.join( 163 | translated_parts_dir, f"{wave[y, x]}_{y}_{x}_{wave_names[wave[y, x]]}_translated.obj" 164 | ) 165 | ) 166 | if overhanging_cfg is not None: 167 | over_mesh.export( 168 | os.path.join( 169 | translated_parts_dir, 170 | f"{over_wave[y, x]}_{y}_{x}_{over_wave_names[over_wave[y, x]]}_translated.obj", 171 | ) 172 | ) 173 | terrain_mesh.export( 174 | os.path.join( 175 | translated_parts_dir, 176 | f"{wave[y, x]}_{y}_{x}_{wave_names[wave[y, x]]}_terrain_translated.obj", 177 | ) 178 | ) 179 | result_mesh += mesh 180 | if overhanging_cfg is not None: 181 | result_terrain_mesh += terrain_mesh 182 | result_overhanging_mesh += over_mesh 183 | bar() 184 | 185 | bbox = result_mesh.bounding_box.bounds 186 | print("bbox = ", bbox) 187 | # Get the center of the bounding box. 188 | center = np.mean(bbox, axis=0) 189 | center[2] = 0.0 190 | # Translate the mesh to the center of the bounding box. 191 | result_mesh = result_mesh.apply_translation(-center) 192 | 193 | print("saving mesh to ", save_dir) 194 | result_mesh.export(os.path.join(save_dir, "mesh.obj")) 195 | # result_mesh.export(save_dir) 196 | if overhanging_cfg is not None: 197 | result_terrain_mesh = result_terrain_mesh.apply_translation(-center) 198 | result_terrain_mesh.export(os.path.join(save_dir, "terrain_mesh.obj")) 199 | # result_terrain_mesh.export(save_dir + "_terrain.obj") 200 | result_overhanging_mesh = result_overhanging_mesh.apply_translation(-center) 201 | result_overhanging_mesh.export(os.path.join(save_dir, "overhanging_mesh.obj")) 202 | # result_overhanging_mesh.export(save_dir + "_overhanging.obj") 203 | 204 | if enable_sdf: 205 | # sdf_name = save_dir + ".npy" 206 | sdf_name = os.path.join(save_dir, "sdf.npy") 207 | print("saving sdf to ", sdf_name) 208 | sdf_tile_n = (np.array(cfg.dim[:2]) / sdf_resolution).astype(int) 209 | sdf = sdf_min[sdf_tile_n[1] : -sdf_tile_n[1], sdf_tile_n[0] : -sdf_tile_n[0], :] 210 | np.save(sdf_name, sdf) 211 | if overhanging_cfg is None: 212 | result_terrain_mesh = result_mesh 213 | spawnable_locations = calc_spawnable_locations_with_sdf( 214 | result_terrain_mesh, sdf_min, height_offset=0.5, sdf_resolution=0.1, sdf_threshold=0.4 215 | ) 216 | # locations_name = save_dir + "spawnable_locations.npy" 217 | locations_name = os.path.join(save_dir, "spawnable_locations.npy") 218 | print("saving locations to ", locations_name) 219 | np.save(locations_name, spawnable_locations) 220 | 221 | # Save as mesh_terrain 222 | mesh_cfg = MeshTerrainCfg(mesh=result_terrain_mesh, sdf=sdf, sdf_resolution=sdf_resolution) 223 | mesh_terrain = MeshTerrain(mesh_cfg) 224 | mesh_terrain.save(save_dir) 225 | if visualize: 226 | visualize_mesh(result_mesh) 227 | 228 | 229 | if __name__ == "__main__": 230 | parser = argparse.ArgumentParser(description="Create mesh from configuration") 231 | parser.add_argument( 232 | "--cfg", 233 | type=str, 234 | choices=["indoor", "overhanging", "overhanging_floor"], 235 | default="indoor", 236 | help="Which configuration to use", 237 | ) 238 | parser.add_argument("--over_cfg", action="store_true", help="Whether to use overhanging configuration") 239 | parser.add_argument("--visualize", action="store_true", help="Whether to visualize the generated mesh") 240 | parser.add_argument("--enable_history", action="store_true", help="Whether to enable mesh history") 241 | parser.add_argument("--enable_sdf", action="store_true", help="Whether to enable sdf") 242 | parser.add_argument("--initial_tile_name", type=str, default="floor", help="Whether to enable sdf") 243 | parser.add_argument( 244 | "--mesh_dir", type=str, default="results/generated_terrain", help="Directory to save the generated mesh files" 245 | ) 246 | parser.add_argument("--mesh_name", type=str, default="mesh", help="Base name of the generated mesh files") 247 | args = parser.parse_args() 248 | 249 | if args.cfg == "indoor": 250 | cfg = IndoorNavigationPatternLevels(wall_height=3.0) 251 | elif args.cfg == "overhanging": 252 | cfg = OverhangingTerrainPattern() 253 | elif args.cfg == "overhanging_floor": 254 | cfg = OverhangingFloorPattern() 255 | else: 256 | raise ValueError(f"Unknown configuration: {args.cfg}") 257 | 258 | if args.over_cfg: 259 | over_cfg = OverhangingPattern() 260 | else: 261 | over_cfg = None 262 | 263 | for i in range(10): 264 | mesh_prefix = f"{args.mesh_name}_{i}" 265 | create_mesh_from_cfg( 266 | cfg, 267 | over_cfg, 268 | prefix=mesh_prefix, 269 | initial_tile_name=args.initial_tile_name, 270 | mesh_dir=args.mesh_dir, 271 | visualize=args.visualize, 272 | enable_history=args.enable_history, 273 | enable_sdf=args.enable_sdf, 274 | ) 275 | -------------------------------------------------------------------------------- /examples/visualize_history.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import os 6 | import glob 7 | import numpy as np 8 | import trimesh 9 | import time 10 | 11 | import open3d as o3d 12 | import argparse 13 | 14 | 15 | def visualize_history( 16 | result_dir: str, 17 | rotate_x: float = 0.0, 18 | rotate_y: float = 0.0, 19 | rotate_z: float = 0.0, 20 | zoom: float = 1.5, 21 | sleep_time: float = 0.05, 22 | export_meshes_in_order: bool = False, 23 | output_dir: str = "mesh_parts", 24 | output_prefix: str = "mesh_part", 25 | ): 26 | wave = np.load(os.path.join(result_dir, "wave.npy"), allow_pickle=True) 27 | wave_order = np.load(os.path.join(result_dir, "wave_order.npy"), allow_pickle=True) 28 | print("wave_order ", wave_order) 29 | 30 | meshes = {} 31 | # all mesh files 32 | for file in glob.glob(os.path.join(result_dir, "translated_parts/*.obj")): 33 | meshes[os.path.basename(file)] = trimesh.load(file) 34 | 35 | if export_meshes_in_order: 36 | output_dir = os.path.join(result_dir, output_dir) 37 | os.makedirs(output_dir, exist_ok=True) 38 | 39 | vis = o3d.visualization.Visualizer() 40 | vis.create_window() 41 | for i in range(wave_order.shape[0] * wave_order.shape[1]): 42 | mesh_idx = np.array(np.where(wave_order == i)).T[0] 43 | mesh_id = wave[mesh_idx[0], mesh_idx[1]] 44 | keywords = ["translated", f"{mesh_id}_{mesh_idx[0]}_{mesh_idx[1]}_"] 45 | for key in meshes.keys(): 46 | if all(x in key for x in keywords): 47 | mesh = meshes[key] 48 | 49 | if export_meshes_in_order: 50 | mesh.export(os.path.join(output_dir, f"{output_prefix}_{i:05d}.obj")) 51 | 52 | o3d_mesh = mesh.as_open3d 53 | R = o3d.geometry.get_rotation_matrix_from_xyz([rotate_x, rotate_y, rotate_z]) 54 | o3d_mesh.rotate(R, center=[0, 0, 0]) 55 | o3d_mesh.compute_vertex_normals() 56 | vis.add_geometry(o3d_mesh) 57 | vis.poll_events() 58 | view_control = vis.get_view_control() 59 | view_control.set_zoom(zoom) 60 | 61 | time.sleep(sleep_time) 62 | time.sleep(1000) 63 | 64 | 65 | if __name__ == "__main__": 66 | parser = argparse.ArgumentParser(description="Visualize mesh history") 67 | parser.add_argument( 68 | "--result_dir", type=str, default="results/test_history_2", help="Directory containing the mesh history files" 69 | ) 70 | parser.add_argument("--rotate_x", type=float, default=-1.0, help="Amount to rotate the mesh around the X axis") 71 | parser.add_argument("--rotate_y", type=float, default=0.0, help="Amount to rotate the mesh around the Y axis") 72 | parser.add_argument("--rotate_z", type=float, default=0.2, help="Amount to rotate the mesh around the Z axis") 73 | parser.add_argument("--zoom", type=float, default=1.5, help=" -- Initial zoom level for the visualization") 74 | parser.add_argument( 75 | "--sleep_time", type=float, default=0.05, help="Amount of time to sleep between each frame during visualization" 76 | ) 77 | parser.add_argument( 78 | "--output_dir", 79 | type=str, 80 | default="mesh_parts", 81 | help="Directory to save the generated mesh parts during visualization", 82 | ) 83 | parser.add_argument( 84 | "--output_prefix", 85 | type=str, 86 | default="mesh_part", 87 | help="Prefix to use for the generated mesh parts during visualization", 88 | ) 89 | parser.add_argument( 90 | "--save", action="store_true", help="enable saving of the generated mesh parts to the output directory" 91 | ) 92 | args = parser.parse_args() 93 | visualize_history( 94 | args.result_dir, 95 | args.rotate_x, 96 | args.rotate_y, 97 | args.rotate_z, 98 | args.zoom, 99 | args.sleep_time, 100 | args.save, 101 | args.output_dir, 102 | args.output_prefix, 103 | ) 104 | -------------------------------------------------------------------------------- /examples/visualize_sdf.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import open3d as o3d 6 | import numpy as np 7 | import trimesh 8 | import matplotlib.pyplot as plt 9 | 10 | from terrain_generator.utils import visualize_mesh_and_sdf_values 11 | from terrain_generator.navigation.mesh_terrain import MeshTerrain, MeshTerrainCfg, SDFArray, NavDistance 12 | 13 | # Load SDF array 14 | sdf_array = np.load("results/overhanging_with_sdf_no_wall/mesh_1.obj.npy") 15 | mesh = trimesh.load("results/overhanging_with_sdf_no_wall/mesh_1.obj") 16 | 17 | sdf = SDFArray(sdf_array, resolution=0.1) 18 | 19 | bbox = mesh.bounds 20 | b_min = np.min(bbox, axis=0) 21 | b_min[1] += 12.0 22 | # b_max = np.max(bbox, axis=0) 23 | b_max = b_min + np.array([30, 0.2, 3.0]) 24 | 25 | print("bbox ", bbox, "b_min", b_min, "b_max", b_max) 26 | # dim = np.array(dim) 27 | # num_elements = np.ceil(np.array(dim) / resolution).astype(int) 28 | xyz_range = [np.linspace(b_min[i], b_max[i], num=int((b_max[i] - b_min[i]) * 50)) for i in range(3)] 29 | query_points = np.stack(np.meshgrid(*xyz_range), axis=-1).astype(np.float32) 30 | query_points = query_points.reshape(-1, 3) 31 | print("query_points", query_points.shape) 32 | 33 | sdf_values = sdf.get_sdf(query_points) 34 | print("sdf_values", sdf_values.shape) 35 | 36 | visualize_mesh_and_sdf_values(mesh, query_points, sdf_values) 37 | 38 | 39 | # visualize_mesh_and_sdf(mesh, sdf_array, 0.1, vmin=0.0, vmax=3.0) 40 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | from setuptools import setup, find_packages 6 | 7 | setup( 8 | name="terrain_generator", 9 | version="0.1", 10 | packages=find_packages(), 11 | install_requires=[ 12 | "numpy>=1.19.5", 13 | "matplotlib>=3.3.3", 14 | "open3d", 15 | "networkx", 16 | "trimesh", 17 | "torch", 18 | "alive-progress", 19 | "ray", 20 | ], 21 | entry_points={ 22 | "console_scripts": [ 23 | "terrain-generator=terrain_generator.cli:main", 24 | ], 25 | }, 26 | package_data={"terrain_generator": ["*.py"]}, 27 | description="A Python library for generating terrain meshes", 28 | author="Takahiro Miki", 29 | author_email="takahiro.miki1992@gmail.com", 30 | url="https://github.com/leggedrobotics/terrain_generator", 31 | ) 32 | -------------------------------------------------------------------------------- /terrain_generator/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/terrain-generator/7be9d9a6ebbed7dc669486426184a4694bfd0bb6/terrain_generator/__init__.py -------------------------------------------------------------------------------- /terrain_generator/navigation/graph_search.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import networkx as nx 6 | import matplotlib.pyplot as plt 7 | import numpy as np 8 | 9 | from ..utils import get_height_array_of_mesh 10 | 11 | 12 | def cost_function(G, source, target): 13 | pass 14 | 15 | 16 | class MeshNavigation(object): 17 | """Class for navigation based on terrain mesh""" 18 | 19 | def __init__(self, mesh): 20 | self.mesh = mesh 21 | self.G = self.create_graph_from_mesh(mesh) 22 | 23 | def create_graph_from_mesh(self, mesh, cost_function=None): 24 | """Create a graph with costs from a mesh""" 25 | bbox = mesh.bounds 26 | 27 | 28 | if __name__ == "__main__": 29 | import numpy as np 30 | import networkx as nx 31 | from scipy.sparse import csr_matrix 32 | from scipy.sparse.csgraph import dijkstra 33 | from scipy.sparse.csgraph import shortest_path 34 | 35 | size = (20, 20) 36 | 37 | # Define cost map with random costs 38 | # cost_map = np.random.randint(1, 10, size=size) 39 | cost_map = np.ones(size) 40 | 41 | cost_map[5, :15] = 100.0 42 | cost_map[10, 5:] = 100.0 43 | cost_map[15, :15] = 100.0 44 | 45 | print("Cost map:") 46 | print(cost_map) 47 | 48 | # Generate grid graph 49 | G = nx.grid_2d_graph(*cost_map.shape) 50 | print("G ", G) 51 | print("G.nodes():", G.nodes()) 52 | # Add cost map to edges 53 | for (u, v) in G.edges(): 54 | print("u, v: ", u, v) 55 | cost = cost_map[u[0], u[1]] + cost_map[v[0], v[1]] 56 | print("cost: ", cost) 57 | G[u][v]["weight"] = cost 58 | 59 | # Compute adjacency matrix 60 | adj_mtx = nx.adjacency_matrix(G) 61 | print("adj_mtx", adj_mtx, adj_mtx.shape) 62 | 63 | graph = csr_matrix(adj_mtx) 64 | 65 | # Compute shortest path distances using Dijkstra's algorith 66 | dist_matrix, predecessors = shortest_path(csgraph=graph, directed=False, return_predecessors=True) 67 | 68 | # dist_mtx, _ = dijkstra(csgraph=adj_mtx, directed=False, return_predecessors=False) 69 | 70 | # Print results 71 | print("Distance matrix:") 72 | print(dist_matrix, dist_matrix.shape) 73 | print("d(10, 25) = ", dist_matrix[10, 25]) 74 | nodes_list = np.array(list(G.nodes())) 75 | print("node (10, 25) = ", nodes_list[10], nodes_list[100]) 76 | dist_from_10 = dist_matrix[150, :] 77 | print("dist_from_10", dist_from_10.shape) 78 | img = dist_from_10.reshape(cost_map.shape) 79 | print("img", img.shape) 80 | plt.imshow(img, vmax=30) 81 | # maximum value in the img is 300 82 | plt.show() 83 | -------------------------------------------------------------------------------- /terrain_generator/test/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/terrain-generator/7be9d9a6ebbed7dc669486426184a4694bfd0bb6/terrain_generator/test/__init__.py -------------------------------------------------------------------------------- /terrain_generator/test/conftest.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import pytest 6 | 7 | 8 | def pytest_addoption(parser): 9 | parser.addoption("--visualize", action="store_true", help="To enable debug visualizer: bool") 10 | parser.addoption("--sdf_path", type=str, default="", help="Path to SDF file: str") 11 | 12 | 13 | @pytest.fixture 14 | def visualize(request): 15 | return request.config.getoption("--visualize") 16 | 17 | 18 | @pytest.fixture 19 | def sdf_path(request): 20 | return request.config.getoption("--sdf_path") 21 | -------------------------------------------------------------------------------- /terrain_generator/test/test_basic_parts.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | import trimesh 7 | from ..trimesh_tiles.mesh_parts.basic_parts import create_capsule_mesh, create_floor 8 | from ..trimesh_tiles.mesh_parts.mesh_parts_cfg import ( 9 | MeshPartsCfg, 10 | PlatformMeshPartsCfg, 11 | HeightMapMeshPartsCfg, 12 | WallMeshPartsCfg, 13 | CapsuleMeshPartsCfg, 14 | ) 15 | from ..utils import ( 16 | merge_meshes, 17 | merge_two_height_meshes, 18 | convert_heightfield_to_trimesh, 19 | merge_two_height_meshes, 20 | get_height_array_of_mesh, 21 | ENGINE, 22 | ) 23 | 24 | 25 | def test_capsule(visualize=False): 26 | positions = [np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.5, 0.0]), np.array([-0.5, 0.0, 0.0])] 27 | transformations = [trimesh.transformations.random_rotation_matrix() for i in range(len(positions))] 28 | for i in range(len(positions)): 29 | transformations[i][:3, -1] = positions[i] 30 | capsule_cfg = CapsuleMeshPartsCfg( 31 | radii=(0.1, 0.2, 0.3), heights=(0.4, 0.3, 0.4), transformations=tuple(transformations) 32 | ) 33 | capsule_mesh = create_capsule_mesh(capsule_cfg) 34 | floor = create_floor(capsule_cfg) 35 | mesh = merge_meshes([floor, capsule_mesh]) 36 | if visualize: 37 | mesh.show() 38 | print(get_height_array_of_mesh(capsule_mesh, capsule_cfg.dim, 5)) 39 | 40 | 41 | def test_rail(visualize=False): 42 | # poles 43 | x = np.linspace(-1.0, 1.0, 10) 44 | positions = [np.array([x[i], 0.0, -1.0 + 0.10]) for i in range(len(x))] 45 | heights = [0.5 for i in range(len(positions))] 46 | radii = [0.02 for i in range(len(positions))] 47 | transformations = [np.eye(4) for i in range(len(positions))] 48 | 49 | # rail 50 | positions.append(np.array([-1.0, 0.0, -0.5 + 0.1])) 51 | heights.append(2.0) 52 | radii.append(0.02) 53 | transformations.append(trimesh.transformations.rotation_matrix(np.pi / 2.0, [0.0, 1.0, 0.0])) 54 | # positions = [np.array([0.0, 0.0, 0.0]), np.array([0.0, 0.5, 0.0]), np.array([-0.5, 0.0, 0.0])] 55 | # transformations = [np.eye(4) for i in range(len(positions))] 56 | for i in range(len(positions)): 57 | transformations[i][:3, -1] = positions[i] 58 | capsule_cfg = CapsuleMeshPartsCfg( 59 | radii=tuple(radii), heights=tuple(heights), transformations=tuple(transformations), minimal_triangles=False 60 | ) 61 | capsule_mesh = create_capsule_mesh(capsule_cfg) 62 | floor = create_floor(capsule_cfg) 63 | mesh = merge_meshes([floor, capsule_mesh], False) 64 | if visualize: 65 | capsule_mesh.show() 66 | print("transformations", transformations) 67 | print(capsule_cfg) 68 | mesh.show() 69 | print(get_height_array_of_mesh(capsule_mesh, capsule_cfg.dim, 5)) 70 | -------------------------------------------------------------------------------- /terrain_generator/test/test_mesh.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | import trimesh 7 | 8 | from ..utils import ( 9 | flip_mesh, 10 | yaw_rotate_mesh, 11 | get_height_array_of_mesh, 12 | merge_meshes, 13 | compute_sdf, 14 | visualize_sdf, 15 | visualize_mesh_and_sdf, 16 | clean_mesh, 17 | ) 18 | 19 | 20 | def test_flip_mesh(): 21 | mesh = trimesh.creation.box([1, 1, 0.1], trimesh.transformations.translation_matrix([0, 0, 0])) 22 | box = trimesh.creation.box([0.5, 0.5, 0.5], trimesh.transformations.translation_matrix([-0.25, -0.25, 0.25])) 23 | mesh = merge_meshes([mesh, box]) 24 | # mesh.show() 25 | 26 | flipped_mesh = flip_mesh(mesh, "x") 27 | # flipped_mesh.show() 28 | assert np.allclose(flipped_mesh.vertices[:, 0], -mesh.vertices[:, 0]) 29 | 30 | flipped_mesh = flip_mesh(mesh, "y") 31 | # flipped_mesh.show() 32 | assert np.allclose(flipped_mesh.vertices[:, 1], -mesh.vertices[:, 1]) 33 | 34 | 35 | def test_rotate_mesh(): 36 | mesh = trimesh.creation.box([1, 1, 0.1], trimesh.transformations.translation_matrix([0, 0, 0])) 37 | box = trimesh.creation.box([0.5, 0.5, 0.5], trimesh.transformations.translation_matrix([-0.25, -0.25, 0.25])) 38 | mesh = merge_meshes([mesh, box]) 39 | # mesh.show() 40 | 41 | rotated_mesh = yaw_rotate_mesh(mesh, 90) 42 | # rotated_mesh.show() 43 | # assert np.allclose(rotated_mesh.vertices[:, 0], mesh.vertices[:, 1]) 44 | # assert np.allclose(rotated_mesh.vertices[:, 1], -mesh.vertices[:, 0]) 45 | 46 | rotated_mesh = yaw_rotate_mesh(mesh, 180) 47 | # rotated_mesh.show() 48 | # assert np.allclose(rotated_mesh.vertices[:, 0], -mesh.vertices[:, 0]) 49 | # assert np.allclose(rotated_mesh.vertices[:, 1], -mesh.vertices[:, 1]) 50 | 51 | rotated_mesh = yaw_rotate_mesh(mesh, 270) 52 | # rotated_mesh.show() 53 | # assert np.allclose(rotated_mesh.vertices[:, 0], -mesh.vertices[:, 1]) 54 | # assert np.allclose(rotated_mesh.vertices[:, 1], mesh.vertices[:, 0]) 55 | 56 | 57 | def test_get_height_array(): 58 | mesh = trimesh.creation.box([1, 1, 0.1], trimesh.transformations.translation_matrix([0, 0, -0.5 + 0.05])) 59 | box = trimesh.creation.box([0.5, 0.5, 0.5], trimesh.transformations.translation_matrix([-0.25, -0.25, -0.25])) 60 | mesh = merge_meshes([mesh, box]) 61 | # mesh.show() 62 | 63 | height_array = get_height_array_of_mesh(mesh, [1, 1, 1], 5) 64 | # print(height_array) 65 | 66 | array = np.array( 67 | [ 68 | [0.1, 0.1, 0.1, 0.1, 0.1], 69 | [0.1, 0.1, 0.1, 0.1, 0.1], 70 | [0.5, 0.5, 0.5, 0.1, 0.1], 71 | [0.5, 0.5, 0.5, 0.1, 0.1], 72 | [0.5, 0.5, 0.5, 0.1, 0.1], 73 | ] 74 | ) 75 | 76 | assert height_array.shape == (5, 5) 77 | assert np.allclose(height_array, array) 78 | 79 | flipped_mesh = flip_mesh(mesh, "x") 80 | height_array = get_height_array_of_mesh(flipped_mesh, [1, 1, 1], 5) 81 | # print(height_array) 82 | 83 | array = np.array( 84 | [ 85 | [0.1, 0.1, 0.1, 0.1, 0.1], 86 | [0.1, 0.1, 0.1, 0.1, 0.1], 87 | [0.1, 0.1, 0.5, 0.5, 0.5], 88 | [0.1, 0.1, 0.5, 0.5, 0.5], 89 | [0.1, 0.1, 0.5, 0.5, 0.5], 90 | ] 91 | ) 92 | assert height_array.shape == (5, 5) 93 | assert np.allclose(height_array, array) 94 | 95 | flipped_mesh = flip_mesh(mesh, "y") 96 | height_array = get_height_array_of_mesh(flipped_mesh, [1, 1, 1], 5) 97 | # print(height_array) 98 | 99 | array = np.array( 100 | [ 101 | [0.5, 0.5, 0.5, 0.1, 0.1], 102 | [0.5, 0.5, 0.5, 0.1, 0.1], 103 | [0.5, 0.5, 0.5, 0.1, 0.1], 104 | [0.1, 0.1, 0.1, 0.1, 0.1], 105 | [0.1, 0.1, 0.1, 0.1, 0.1], 106 | ] 107 | ) 108 | assert height_array.shape == (5, 5) 109 | assert np.allclose(height_array, array) 110 | 111 | 112 | def test_compute_sdf(visualize=False): 113 | box = trimesh.creation.box([0.5, 0.5, 0.5], trimesh.transformations.translation_matrix([0.0, 0.0, 0.0])) 114 | box2 = trimesh.creation.box([0.3, 0.3, 0.3], trimesh.transformations.translation_matrix([0.2, -1.0, -0.00])) 115 | box3 = trimesh.creation.box([0.2, 0.2, 0.2], trimesh.transformations.translation_matrix([0.0, 0.35, 0.0])) 116 | box4 = trimesh.creation.box([0.2, 0.2, 0.2], trimesh.transformations.translation_matrix([0.3, -0.30, 0.35])) 117 | box += box2 + box3 + box4 118 | sdf = compute_sdf(mesh=box, dim=[2, 2, 1], resolution=0.1) 119 | if visualize: 120 | box.show() 121 | visualize_sdf(sdf) 122 | visualize_mesh_and_sdf(box, sdf, voxel_size=0.1) 123 | 124 | 125 | def test_clean_mesh(visualize=True): 126 | box = trimesh.creation.box([0.5, 0.5, 0.5], trimesh.transformations.translation_matrix([0.0, 0.0, 0.0])) 127 | box2 = trimesh.creation.box([0.5, 0.5, 0.5], trimesh.transformations.translation_matrix([0.25, 0.0, 0.0])) 128 | # box3 = trimesh.creation.box([0.2, 0.2, 0.2], trimesh.transformations.translation_matrix([0.0, 0.35, 0.0])) 129 | # box4 = trimesh.creation.box([0.2, 0.2, 0.2], trimesh.transformations.translation_matrix([0.3, -0.30, 0.35])) 130 | box += box2 131 | 132 | cleaned_box = clean_mesh(box) 133 | if visualize: 134 | print("box vertices: ", box.vertices.shape, "faces: ", box.faces.shape) 135 | print("cleaned_box vertices: ", cleaned_box.vertices.shape, "faces: ", cleaned_box.faces.shape) 136 | box.show() 137 | # sdf = compute_sdf(mesh=box, dim=[2, 2, 1], resolution=0.1) 138 | 139 | 140 | # def test_visualize_sdf(sdf_path): 141 | # sdf = np.load(sdf_path) 142 | # visualize_sdf(sdf) 143 | -------------------------------------------------------------------------------- /terrain_generator/test/test_mesh_terrain.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | import torch 7 | import trimesh 8 | 9 | from ..utils import ( 10 | calc_spawnable_locations_on_terrain, 11 | locations_to_graph, 12 | visualize_mesh_and_graphs, 13 | filter_spawnable_locations_with_sdf, 14 | get_height_array_of_mesh_with_resolution, 15 | distance_matrix_from_graph, 16 | create_2d_graph_from_height_array, 17 | visualize_distance, 18 | ) 19 | 20 | from ..navigation.mesh_terrain import MeshTerrain, MeshTerrainCfg, SDFArray, NavDistance 21 | 22 | 23 | def test_mesh_terrain_transform(visualize): 24 | nav_mesh = MeshTerrain(MeshTerrainCfg()) 25 | translation = torch.Tensor([1.0, 2.0, 3.0]) 26 | 27 | # translation 28 | nav_mesh.translate(translation) 29 | assert np.allclose(nav_mesh.sdf.center.cpu().numpy(), translation, atol=1e-5) 30 | assert np.allclose(nav_mesh.nav_distance.center.cpu().numpy(), translation[:2], rtol=1e-3, atol=1e-5) 31 | 32 | # transform 33 | transform = np.eye(4) 34 | transform[:3, 3] = translation 35 | nav_mesh.transform(transform) 36 | assert np.allclose(nav_mesh.sdf.center, translation * 2, atol=1e-5) 37 | assert np.allclose(nav_mesh.nav_distance.center, translation[:2] * 2, rtol=1e-3, atol=1e-5) 38 | -------------------------------------------------------------------------------- /terrain_generator/test/test_nav_utils.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | import torch 7 | import trimesh 8 | 9 | from ..trimesh_tiles.mesh_parts.create_tiles import create_mesh_tile, get_mesh_gen 10 | from ..trimesh_tiles.mesh_parts.mesh_parts_cfg import FloatingBoxesPartsCfg, WallMeshPartsCfg, PlatformMeshPartsCfg 11 | from ..trimesh_tiles.mesh_parts.overhanging_parts import create_overhanging_boxes, generate_wall_from_array 12 | 13 | from ..utils import ( 14 | calc_spawnable_locations_on_terrain, 15 | locations_to_graph, 16 | visualize_mesh_and_graphs, 17 | filter_spawnable_locations_with_sdf, 18 | get_height_array_of_mesh_with_resolution, 19 | distance_matrix_from_graph, 20 | create_2d_graph_from_height_array, 21 | visualize_distance, 22 | ) 23 | 24 | from ..navigation.mesh_terrain import MeshTerrain, MeshTerrainCfg, SDFArray, NavDistance 25 | 26 | 27 | def test_spawnable_location(visualize=False): 28 | # Load SDF array 29 | sdf_array = np.load("results/overhanging_with_sdf_no_wall/mesh_0.obj.npy") 30 | terrain_mesh = trimesh.load("results/overhanging_with_sdf_no_wall/mesh_0.obj_terrain.obj") 31 | all_mesh = trimesh.load("results/overhanging_with_sdf_no_wall/mesh_0.obj") 32 | spawnable_locations = calc_spawnable_locations_on_terrain( 33 | terrain_mesh, 34 | filter_size=(5, 5), 35 | visualize=False, 36 | resolution=0.1, 37 | ) 38 | if visualize: 39 | print("spawnable_locations", spawnable_locations, spawnable_locations.shape) 40 | new_spawnable_locations = filter_spawnable_locations_with_sdf( 41 | spawnable_locations, sdf_array, height_offset=0.5, sdf_resolution=0.1, sdf_threshold=0.4 42 | ) 43 | 44 | # sampled_points = spawnable_locations[np.random.choice(spawnable_locations.shape[0], 1000)] 45 | print("new spawnable_locations", new_spawnable_locations, new_spawnable_locations.shape) 46 | # graph = locations_to_graph(new_spawnable_locations) 47 | visualize_mesh_and_graphs(all_mesh, new_spawnable_locations) 48 | 49 | 50 | def test_create_2d_graph_from_height_array(visualize=False, load_mesh=True): 51 | # import numpy as np 52 | import networkx as nx 53 | from scipy.sparse import csr_matrix 54 | from scipy.sparse.csgraph import shortest_path 55 | import matplotlib.pyplot as plt 56 | 57 | if load_mesh: 58 | mesh = trimesh.load("results/overhanging_with_sdf_no_wall/mesh_0.obj_terrain.obj") 59 | height_array, _ = get_height_array_of_mesh_with_resolution(mesh, resolution=0.1) 60 | else: 61 | height_array = np.zeros((80, 80)) 62 | height_array[20, :60] = 2.0 63 | height_array[40, 20:] = 2.0 64 | height_array[60, :50] = 2.0 65 | 66 | G = create_2d_graph_from_height_array(height_array, graph_ratio=4) 67 | 68 | if visualize: 69 | plt.imshow(height_array) 70 | plt.show() 71 | grid_size = np.array(height_array.shape) // 4 72 | 73 | # Compute adjacency matrix 74 | adj_mtx = nx.adjacency_matrix(G) 75 | print("adj_mtx", adj_mtx, adj_mtx.shape) 76 | 77 | graph = csr_matrix(adj_mtx) 78 | 79 | # Compute shortest path distances using Dijkstra's algorith 80 | dist_matrix, predecessors = shortest_path(csgraph=graph, directed=False, return_predecessors=True) 81 | 82 | np.save("results/overhanging_with_sdf_no_wall/dist_matrix.npy", dist_matrix) 83 | np.save("results/overhanging_with_sdf_no_wall/height_map.npy", height_array) 84 | 85 | # dist_mtx, _ = dijkstra(csgraph=adj_mtx, directed=False, return_predecessors=False) 86 | 87 | # Print results 88 | print("Distance matrix:") 89 | print(dist_matrix, dist_matrix.shape) 90 | print("d(10, 25) = ", dist_matrix[10, 25]) 91 | nodes_list = np.array(list(G.nodes())) 92 | print("node (10, 25) = ", nodes_list[10], nodes_list[100]) 93 | dist_from_10 = dist_matrix[50, :] 94 | print("dist_from_10", dist_from_10.shape) 95 | img = dist_from_10.reshape(grid_size) 96 | print("img", img.shape) 97 | plt.imshow(img, vmax=100) 98 | # maximum value in the img is 300 99 | plt.show() 100 | visualize_distance(height_array, dist_matrix, 4, (60, 30), height_array_resolution=0.1) 101 | 102 | 103 | def test_visualize_distance(visualize=False): 104 | mesh = trimesh.load("results/overhanging_with_sdf_no_wall/mesh_0.obj_terrain.obj") 105 | height_array = np.load("results/overhanging_with_sdf_no_wall/height_map.npy") 106 | dist_matrix = np.load("results/overhanging_with_sdf_no_wall/dist_matrix.npy") 107 | ratio = 4 108 | if visualize: 109 | visualize_distance(height_array, dist_matrix, ratio, (20, 80), height_array_resolution=0.1) 110 | 111 | 112 | def test_save_nav_mesh(visualize=False): 113 | cfg = MeshTerrainCfg( 114 | mesh_path="results/overhanging_with_sdf_no_wall/mesh_0.obj_terrain.obj", 115 | # mesh_path="results/overhanging_with_sdf_no_wall/tree.obj", 116 | # distance_path="results/overhanging_with_sdf_no_wall/dist_matrix.npy", 117 | # sdf_path="results/overhanging_with_sdf_no_wall/mesh_0.obj.npy", 118 | ) 119 | # print("cfg", cfg) 120 | mesh_terrain = MeshTerrain(cfg) 121 | # print("mesh_terrain", mesh_terrain) 122 | mesh_terrain.save("results/overhanging_with_sdf_no_wall/mesh_terrain_0") 123 | 124 | 125 | def test_load_nav_mesh(visualize): 126 | # cfg = MeshTerrainCfg( 127 | # mesh_path="results/overhanging_with_sdf_no_wall/mesh_0.obj_terrain.obj", 128 | # # distance_path="results/overhanging_with_sdf_no_wall/dist_matrix.npy", 129 | # # sdf_path="results/overhanging_with_sdf_no_wall/mesh_0.obj.npy", 130 | # ) 131 | cfg_path = "results/overhanging/mesh_0/mesh_terrain.json" 132 | mesh_terrain = MeshTerrain(cfg_path, device="cuda:0") 133 | # sdf_points = 134 | # mesh_terrain.save_to_file("results/overhanging_with_sdf_no_wall/mesh_terrain") 135 | # x = np.linspace(-19.99, 19.99, 200) 136 | # y = np.linspace(-19.99, 19.99, 200) 137 | # xv, yv = np.meshgrid(x, y) 138 | # points = np.stack([xv, yv], axis=2).reshape(-1, 2) 139 | array, center, mesh_points = get_height_array_of_mesh_with_resolution( 140 | mesh_terrain.mesh, resolution=0.05, return_points=True 141 | ) 142 | goal_pos = torch.Tensor( 143 | [ 144 | [-11.23, 10.26], 145 | [-10.00, 10.00], 146 | [0.0, 0.0], 147 | [0.2, 0.4], 148 | [10.0, 0.0], 149 | [10.23, 0.234], 150 | [-10.0, 0.0], 151 | [0.0, 10.0], 152 | [0.0, -10.0], 153 | [10, 10], 154 | [-10, -10], 155 | ] 156 | ) 157 | # goal_pos = torch.Tensor([[-5.0, 12.0]]) 158 | d = mesh_terrain.get_distance(mesh_points[:, :2].copy(), goal_pos) 159 | print("d ", d.shape) 160 | d = d.clip(0, 200) 161 | if visualize: 162 | print("center ", center) 163 | for i in range(d.shape[0]): 164 | print(i, goal_pos[i]) 165 | visualize_mesh_and_graphs(mesh_terrain.mesh, mesh_points, color_values=d[i], goal_pos=goal_pos[i]) 166 | # plt.imshow(d[i, :, :]) 167 | # plt.show() 168 | 169 | # import matplotlib.pyplot as plt 170 | # plt.imshow(array) 171 | # plt.show() 172 | # plt.imshow(d.reshape(array.shape)) 173 | # plt.show() 174 | 175 | 176 | def test_sdf_array(visualize=False): 177 | sdf_array = torch.Tensor([[[1.0, 2, 3], [4, 5, 6], [7, 8, 9]]]).reshape(3, 3, 1) 178 | sdf_array = SDFArray(sdf_array, resolution=0.1) 179 | 180 | points = torch.Tensor([[0.0, 0.0, 0.0], [0.1, 0.1, 0.0], [0.05, 0.05, 0.00]]) 181 | sdf = sdf_array.get_sdf(points) 182 | if visualize: 183 | print("sdf_array", sdf_array) 184 | print("sdf", sdf) 185 | 186 | expected_values = torch.Tensor([5.0, 9.0, 7.0]) 187 | assert torch.allclose(sdf, expected_values) 188 | 189 | 190 | def test_nav_distance(visualize): 191 | height_array = np.zeros((80, 100)) 192 | height_array[20, :60] = 2.0 193 | height_array[40, 20:] = 2.0 194 | height_array[60, :50] = 2.0 195 | G = create_2d_graph_from_height_array(height_array, graph_ratio=4, invalid_cost=1000) 196 | dist_matrix = distance_matrix_from_graph(G) 197 | 198 | nav_distance = NavDistance(dist_matrix, shape=(20, 25), resolution=0.4) 199 | 200 | # points = torch.Tensor([[0.0, 0.0], [0.1, 0.1], [0.05, 0.05]]) 201 | # get points from mesh grid 202 | x = np.linspace(-3.99, 3.99, 200) 203 | y = np.linspace(-3.99, 3.99, 200) 204 | xv, yv = np.meshgrid(x, y) 205 | points = np.stack([xv, yv], axis=2).reshape(-1, 2) 206 | goal_pos = torch.Tensor([[0.0, -1.5]]) 207 | d = nav_distance.get_distance(points, goal_pos) 208 | if visualize: 209 | print("dist_matrix", dist_matrix.shape) 210 | print("nav_distance", nav_distance) 211 | import matplotlib.pyplot as plt 212 | 213 | # first visualize the distance map 214 | distance_map = dist_matrix[120, :].reshape((20, 25)) 215 | plt.imshow(distance_map, vmax=300) 216 | plt.show() 217 | 218 | print("points", points, points.shape) 219 | print("d", d, d.shape) 220 | img = d.reshape((200, 200)) 221 | plt.imshow(img, vmax=300) 222 | plt.show() 223 | 224 | # expected_values = torch.Tensor([5.0, 9.0, 7.0]) 225 | # assert torch.allclose(distances, expected_values) 226 | 227 | 228 | def test_nav_batched_distance(visualize): 229 | height_array = np.zeros((80, 100)) 230 | height_array[20, :60] = 2.0 231 | height_array[40, 20:] = 2.0 232 | height_array[60, :50] = 2.0 233 | G = create_2d_graph_from_height_array(height_array, graph_ratio=4, invalid_cost=1000) 234 | dist_matrix = distance_matrix_from_graph(G) 235 | 236 | nav_distance = NavDistance(dist_matrix, shape=(20, 25), resolution=0.4) 237 | 238 | # points = torch.Tensor([[0.0, 0.0], [0.1, 0.1], [0.05, 0.05]]) 239 | # get points from mesh grid 240 | x = np.linspace(-4.00, 4.00, 2) 241 | y = np.linspace(-4.00, 4.00, 2) 242 | xv, yv = np.meshgrid(x, y) 243 | points = np.stack([xv, yv], axis=2).reshape(-1, 2) 244 | 245 | goal_pos = torch.Tensor([[0.0, -1.5]]) 246 | points = torch.from_numpy(points).float() 247 | d = nav_distance.get_distance(points, goal_pos) 248 | expected_d = torch.Tensor([[97.94102602, 57.94106602, 1000.0, 1000.0]]) 249 | assert torch.allclose(d, expected_d) 250 | 251 | goal_pos = torch.Tensor([[0.0, -1.5]]) 252 | points_shifted = points.clone() + 1.0 253 | d = nav_distance.get_distance(points_shifted, goal_pos) 254 | expected_d = torch.Tensor([[555.7985, 63.7989, 1000.0, 1000.0]]) 255 | assert torch.allclose(d, expected_d) 256 | 257 | goal_pos = torch.Tensor([[0.0, 1.5]]) 258 | d = nav_distance.get_distance(points, goal_pos) 259 | expected_d = torch.Tensor([[76.9705, 32.2842, 1000.0, 1000.0]]) 260 | assert torch.allclose(d, expected_d) 261 | 262 | # Batched goal pos 263 | goal_pos = torch.Tensor([[0.0, -1.5], [0.0, 1.5]]) 264 | d = nav_distance.get_distance(points, goal_pos) 265 | expected_d = torch.Tensor([[97.94102602, 57.94106602, 1000.0, 1000.0], [76.9705, 32.2842, 1000.0, 1000.0]]) 266 | assert torch.allclose(d, expected_d) 267 | 268 | # Batched goal pos and points 269 | goal_pos = torch.Tensor([[0.0, 1.5], [0.0, -1.5]]) 270 | points = torch.stack([points, points_shifted], dim=0) 271 | d = nav_distance.get_distance(points, goal_pos) 272 | expected_d = torch.Tensor([[76.9705, 32.2842, 1000.0, 1000.0], [555.7985, 63.7989, 1000.0, 1000.0]]) 273 | assert torch.allclose(d, expected_d) 274 | 275 | if visualize: 276 | print("d ", d) 277 | print("dist_matrix", dist_matrix.shape) 278 | print("nav_distance", nav_distance) 279 | import matplotlib.pyplot as plt 280 | 281 | # first visualize the distance map 282 | distance_map = dist_matrix[120, :].reshape((20, 25)) 283 | plt.imshow(distance_map, vmax=300) 284 | plt.show() 285 | 286 | print("points", points, points.shape) 287 | print("d", d, d.shape) 288 | img = d[0].reshape((2, 2)) 289 | plt.imshow(img, vmax=300) 290 | plt.show() 291 | 292 | # expected_values = torch.Tensor([5.0, 9.0, 7.0]) 293 | # assert torch.allclose(distances, expected_values) 294 | -------------------------------------------------------------------------------- /terrain_generator/test/test_overhanging.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | import trimesh 7 | 8 | from ..trimesh_tiles.mesh_parts.create_tiles import create_mesh_tile, get_mesh_gen 9 | from ..trimesh_tiles.mesh_parts.mesh_parts_cfg import ( 10 | OverhangingBoxesPartsCfg, 11 | WallMeshPartsCfg, 12 | PlatformMeshPartsCfg, 13 | FloatingBoxesPartsCfg, 14 | ) 15 | from ..trimesh_tiles.mesh_parts.overhanging_parts import ( 16 | create_overhanging_boxes, 17 | generate_wall_from_array, 18 | create_floating_boxes, 19 | ) 20 | 21 | from ..utils import get_height_array_of_mesh 22 | 23 | 24 | def test_generate_wall_from_array(visualize=False): 25 | array = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]]) 26 | cfg = WallMeshPartsCfg(connection_array=array, wall_thickness=0.1, wall_height=2.0) 27 | mesh = generate_wall_from_array(cfg) 28 | 29 | height_array = (get_height_array_of_mesh(mesh, cfg.dim, 3) > 0).astype(np.float32) 30 | assert np.allclose(height_array, array) 31 | if visualize: 32 | print("array ", array) 33 | print("height array ", height_array) 34 | # mesh.show() 35 | 36 | array = np.array([[0, 1, 0], [0, 1, 1], [0, 0, 0]]) 37 | cfg = WallMeshPartsCfg(connection_array=array, wall_thickness=0.1, wall_height=2.0) 38 | mesh = generate_wall_from_array(cfg) 39 | 40 | height_array = (get_height_array_of_mesh(mesh, cfg.dim, 3) > 0).astype(np.float32) 41 | assert np.allclose(height_array, array) 42 | if visualize: 43 | print("array ", array) 44 | print("height array ", height_array) 45 | mesh.show() 46 | 47 | array = np.array([[0, 1, 0], [0, 1, 1], [0, 1, 0]]) 48 | cfg = WallMeshPartsCfg(connection_array=array, wall_thickness=0.4, wall_height=3.0) 49 | mesh = generate_wall_from_array(cfg) 50 | height_array = (get_height_array_of_mesh(mesh, cfg.dim, 3) > 0).astype(np.float32) 51 | assert np.allclose(height_array, array) 52 | 53 | if visualize: 54 | print("array ", array) 55 | print("height array ", height_array) 56 | mesh.show() 57 | 58 | array = np.array([[0, 1, 0], [0, 1, 0], [0, 1, 0]]) 59 | cfg = WallMeshPartsCfg(connection_array=array, wall_thickness=0.6, wall_height=1.0) 60 | mesh = generate_wall_from_array(cfg) 61 | height_array = (get_height_array_of_mesh(mesh, cfg.dim, 3) > 0).astype(np.float32) 62 | assert np.allclose(height_array, array) 63 | 64 | if visualize: 65 | print("array ", array) 66 | print("height array ", height_array) 67 | mesh.show() 68 | 69 | array = np.array([[0, 0, 0], [1, 1, 0], [0, 1, 0]]) 70 | cfg = WallMeshPartsCfg(connection_array=array, wall_thickness=0.2, wall_height=0.1) 71 | mesh = generate_wall_from_array(cfg) 72 | height_array = (get_height_array_of_mesh(mesh, cfg.dim, 3) > 0).astype(np.float32) 73 | assert np.allclose(height_array, array) 74 | 75 | if visualize: 76 | print("array ", array) 77 | print("height array ", height_array) 78 | mesh.show() 79 | 80 | array = np.array([[0, 1, 0], [1, 1, 1], [0, 1, 0]]) 81 | cfg = WallMeshPartsCfg(connection_array=array, wall_thickness=0.4, wall_height=3.0) 82 | mesh = generate_wall_from_array(cfg) 83 | 84 | height_array = (get_height_array_of_mesh(mesh, cfg.dim, 3) > 0).astype(np.float32) 85 | assert np.allclose(height_array, array) 86 | 87 | if visualize: 88 | print("array ", array) 89 | print("height array ", height_array) 90 | mesh.show() 91 | 92 | # array = np.array([[1, 0, 1], [1, 0, 1], [1, 0, 1]]) 93 | # cfg = WallMeshPartsCfg(connection_array=array, wall_thickness=0.4, wall_height=3.0) 94 | # mesh = generate_wall_from_array(cfg) 95 | # 96 | # height_array = (get_height_array_of_mesh(mesh, cfg.dim, 3) > 0).astype(np.float32) 97 | # 98 | # if visualize: 99 | # print("array ", array) 100 | # print("height array ", height_array) 101 | # mesh.show() 102 | # assert np.allclose(height_array, array) 103 | 104 | 105 | def test_overhanging_boxes(visualize=False): 106 | n = 6 107 | height_diff = 0.5 108 | array = np.zeros((n, n)) 109 | array[:] = np.linspace(0, height_diff, n) 110 | array = array.T 111 | array[1:-1, 1:-1] += np.random.normal(0, 0.1, size=[array.shape[0] - 2, array.shape[1] - 2]) 112 | # array[5, :] = height_diff 113 | cfg = PlatformMeshPartsCfg( 114 | name="floor", 115 | array=array, 116 | flips=(), 117 | weight=0.1, 118 | minimal_triangles=False, 119 | ) 120 | mesh_gen = get_mesh_gen(cfg) 121 | mesh = mesh_gen(cfg) 122 | cfg = OverhangingBoxesPartsCfg( 123 | name="floating_boxes", mesh=mesh, gap_mean=0.8, gap_std=0.1, box_grid_n=4, box_height=0.6 124 | ) 125 | box_cfg = create_overhanging_boxes(cfg) 126 | box_gen = get_mesh_gen(box_cfg) 127 | box_mesh = box_gen(box_cfg) 128 | mesh += box_mesh 129 | if visualize: 130 | mesh.show() 131 | 132 | 133 | def test_floating_boxes(visualize=True): 134 | n = 6 135 | height_diff = 0.2 136 | array = np.zeros((n, n)) 137 | array[:] = np.linspace(0, height_diff, n) 138 | array = array.T 139 | array[1:-1, 1:-1] += np.random.normal(0, 0.1, size=[array.shape[0] - 2, array.shape[1] - 2]) 140 | # array[5, :] = height_diff 141 | cfg = PlatformMeshPartsCfg( 142 | name="floor", 143 | array=array, 144 | flips=(), 145 | weight=0.1, 146 | minimal_triangles=False, 147 | ) 148 | mesh_gen = get_mesh_gen(cfg) 149 | mesh = mesh_gen(cfg) 150 | cfg = FloatingBoxesPartsCfg( 151 | name="floating_boxes", 152 | mesh=mesh, 153 | n_boxes=2, 154 | box_dim_min=(1.0, 0.1, 0.1), 155 | box_dim_max=(1.6, 0.1, 0.1), 156 | roll_pitch_range=(0.0, np.pi / 6), # in rad 157 | min_height=0.7, 158 | ) 159 | box_cfg = create_floating_boxes(cfg) 160 | box_gen = get_mesh_gen(box_cfg) 161 | box_mesh = box_gen(box_cfg) 162 | mesh += box_mesh 163 | if visualize: 164 | mesh.show() 165 | -------------------------------------------------------------------------------- /terrain_generator/test/test_tiles.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | 7 | import sys 8 | 9 | from ..wfc.tiles import Tile, ArrayTile, MeshTile 10 | 11 | 12 | def test_tile(): 13 | tile = Tile(name="tile1", edges={"up": "edge1", "right": "edge2", "down": "edge3", "left": "edge4"}) 14 | print(tile) 15 | assert tile.name == "tile1" 16 | 17 | rotated_tile = tile.get_rotated_tile(90) 18 | print("rotated_tile", rotated_tile) 19 | 20 | flipped_tile = tile.get_flipped_tile("x") 21 | print("flipped", flipped_tile) 22 | 23 | tiles = tile.get_all_tiles(rotations=(90, 180, 270), flips=("x", "y")) 24 | for tile in tiles: 25 | print(tile) 26 | 27 | 28 | def test_array_tile(): 29 | tile = ArrayTile(name="tile", array=np.array([[0, 1, 2], [3, 4, 5], [6, 7, 8]])) 30 | print(tile) 31 | 32 | rotated_tile = tile.get_rotated_tile(90) 33 | print("rotated_tile", rotated_tile) 34 | assert np.allclose(rotated_tile.array, np.array([[2, 5, 8], [1, 4, 7], [0, 3, 6]])) 35 | assert rotated_tile.edges["up"] == tile.edges["right"] 36 | assert rotated_tile.edges["right"] == tile.edges["down"] 37 | assert rotated_tile.edges["down"] == tile.edges["left"] 38 | assert rotated_tile.edges["left"] == tile.edges["up"] 39 | 40 | rotated_tile = tile.get_rotated_tile(270) 41 | print("rotated_tile", rotated_tile) 42 | assert np.allclose(rotated_tile.array, np.array([[6, 3, 0], [7, 4, 1], [8, 5, 2]])) 43 | assert rotated_tile.edges["right"] == tile.edges["up"] 44 | assert rotated_tile.edges["down"] == tile.edges["right"] 45 | assert rotated_tile.edges["left"] == tile.edges["down"] 46 | assert rotated_tile.edges["up"] == tile.edges["left"] 47 | 48 | flipped_tile = tile.get_flipped_tile("x") 49 | print("flipped", flipped_tile) 50 | assert np.allclose(flipped_tile.array, np.array([[2, 1, 0], [5, 4, 3], [8, 7, 6]])) 51 | assert flipped_tile.edges["up"] == tile.edges["up"][::-1] 52 | assert flipped_tile.edges["right"] == tile.edges["left"][::-1] 53 | assert flipped_tile.edges["down"] == tile.edges["down"][::-1] 54 | assert flipped_tile.edges["left"] == tile.edges["right"][::-1] 55 | 56 | flipped_tile = tile.get_flipped_tile("y") 57 | print("flipped", flipped_tile) 58 | assert np.allclose(flipped_tile.array, np.array([[6, 7, 8], [3, 4, 5], [0, 1, 2]])) 59 | assert flipped_tile.edges["up"] == tile.edges["down"][::-1] 60 | assert flipped_tile.edges["right"] == tile.edges["right"][::-1] 61 | assert flipped_tile.edges["down"] == tile.edges["up"][::-1] 62 | assert flipped_tile.edges["left"] == tile.edges["left"][::-1] 63 | -------------------------------------------------------------------------------- /terrain_generator/test/test_utils.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | import torch 7 | import trimesh 8 | import time 9 | 10 | from ..utils import sample_interpolated, sample_interpolated_bilinear 11 | 12 | 13 | def test_interpolated_sampling(visualize=True): 14 | 15 | # 2D array 16 | array = torch.Tensor([[[1, 2, 3], [4, 5, 6], [7, 8, 9]]]).reshape(1, 1, 3, 3) 17 | points = torch.Tensor( 18 | [ 19 | [0.0, 0.0], 20 | [1.0, 1.0], 21 | [0.5, 0.5], 22 | [2.0, 1.0], 23 | [1.5, 1.0], 24 | [0.1, 0.0], 25 | [100.0, 0.0], # Outside 26 | ] 27 | ).reshape(1, -1, 1, 2) 28 | values = sample_interpolated(array, points, invalid_value=0.0).flatten() 29 | expected_values = torch.Tensor([1.0, 5.0, 3.0, 8.0, 6.5, 1.3, 0.0]) 30 | print("value ", values) 31 | print("expected value ", expected_values) 32 | 33 | assert torch.allclose(values, expected_values) 34 | 35 | # 3D array 36 | device = torch.device("cuda:0") if torch.cuda.is_available() else torch.device("cpu") 37 | array = ( 38 | torch.Tensor([[[1, 2, 3], [4, 5, 6], [7, 8, 9]], [[10, 10, 10], [10, 21, 30], [40, 50, 60]]]) 39 | .reshape(1, 1, 2, 3, 3) 40 | .to(device) 41 | ) 42 | points = ( 43 | torch.Tensor( 44 | [ 45 | [0.0, 0.0, 0.0], 46 | [0.5, 0.5, 0.0], 47 | [1.0, 1.0, 0.0], 48 | [0.0, 0.0, 1.0], 49 | [0.0, 0.0, 0.5], 50 | [1.0, 1.0, 1.0], 51 | [0.5, 0.5, 0.5], 52 | [0.10, 0.10, 0.10], 53 | [2.0, 0.0, 0.0], # Outside 54 | ] 55 | ) 56 | .to(device) 57 | .reshape(1, -1, 1, 1, 3) 58 | ) 59 | values = sample_interpolated(array, points, invalid_value=100.0).flatten() 60 | expected_values = torch.Tensor([1.0, 6.250, 10.0, 2.0, 1.5, 21.0, 7.8750, 2.2710, 100.0]).to(device) 61 | print("value ", values) 62 | print("expected value ", expected_values) 63 | assert torch.allclose(values, expected_values) 64 | # assert values.device == device 65 | 66 | # numpy 67 | array = np.array([[[1, 2, 3], [4, 5, 6], [7, 8, 9.0]]]).reshape(1, 1, 3, 3) 68 | points = np.array( 69 | [ 70 | [0.0, 0.0], 71 | [1.0, 1.0], 72 | [0.5, 0.5], 73 | [2.0, 1.0], 74 | [1.5, 1.0], 75 | [0.1, 0.0], 76 | [100.0, 0.0], # Outside 77 | ] 78 | ).reshape(1, -1, 1, 2) 79 | values = sample_interpolated(array, points).flatten() 80 | expected_values = np.array([1.0, 5.0, 3.0, 8.0, 6.5, 1.3, 0.0]) 81 | assert np.allclose(values, expected_values) 82 | 83 | # Large 2D array 84 | array = np.arange(400).reshape(1, 1, 20, 20).astype(float) 85 | x = np.linspace(0, 19, 100) 86 | y = np.linspace(0, 19, 100) 87 | xx, yy = np.meshgrid(x, y) 88 | points = np.stack([yy, xx], axis=-1).reshape(1, 100, 100, 2) 89 | s = time.time() 90 | values = sample_interpolated(array, points).flatten() 91 | if visualize: 92 | import matplotlib.pyplot as plt 93 | 94 | plt.imshow(array.reshape(20, 20)) 95 | plt.show() 96 | print(points, values) 97 | 98 | plt.imshow(values.reshape(100, 100)) 99 | plt.show() 100 | 101 | 102 | def test_interpolated_sampling_bilinear(visualize=False): 103 | 104 | # 2D array 105 | array = torch.Tensor([[[1, 2, 3], [4, 5, 6], [7, 8, 9]]]).reshape(3, 3) 106 | points = torch.Tensor( 107 | [ 108 | [0.0, 0.0], 109 | [1.0, 1.0], 110 | [0.5, 0.5], 111 | [2.0, 1.0], 112 | [1.5, 1.0], 113 | [0.1, 0.0], 114 | [100.0, 0.0], # Outside 115 | ] 116 | ) 117 | values = sample_interpolated_bilinear(array, points) 118 | expected_values = torch.Tensor([1.0, 5.0, 3.0, 8.0, 6.5, 1.3, 0.0]) 119 | 120 | assert torch.allclose(values, expected_values) 121 | 122 | # 3D array 123 | device = torch.device("cuda:0") if torch.cuda.is_available() else torch.device("cpu") 124 | array = ( 125 | torch.Tensor([[[1, 2, 3], [4, 5, 6], [7, 8, 9]], [[10, 10, 10], [10, 21, 30], [40, 50, 60]]]) 126 | .reshape(2, 3, 3) 127 | .to(device) 128 | ) 129 | points = torch.Tensor( 130 | [ 131 | [0.0, 0.0, 0.0], 132 | [0.5, 0.5, 0.0], 133 | [1.0, 1.0, 0.0], 134 | [0.0, 0.0, 1.0], 135 | [0.0, 0.0, 0.5], 136 | [1.0, 1.0, 1.0], 137 | [0.5, 0.5, 0.5], 138 | [0.10, 0.10, 0.10], 139 | [2.0, 0.0, 0.0], # Outside 140 | ] 141 | ).to(device) 142 | values = sample_interpolated_bilinear(array, points, invalid_value=100) 143 | expected_values = torch.Tensor([1.0, 6.250, 10.0, 2.0, 1.5, 21.0, 7.8750, 2.2710, 100.0]).to(device) 144 | assert torch.allclose(values, expected_values) 145 | assert values.device == device 146 | 147 | # numpy 148 | array = np.array([[[1, 2, 3], [4, 5, 6], [7, 8, 9]]]).reshape(3, 3) 149 | points = np.array( 150 | [ 151 | [0.0, 0.0], 152 | [1.0, 1.0], 153 | [0.5, 0.5], 154 | [2.0, 1.0], 155 | [1.5, 1.0], 156 | [0.1, 0.0], 157 | [100.0, 0.0], # Outside 158 | ] 159 | ) 160 | values = sample_interpolated_bilinear(array, points) 161 | expected_values = np.array([1.0, 5.0, 3.0, 8.0, 6.5, 1.3, 0.0]) 162 | assert np.allclose(values, expected_values) 163 | 164 | # Large 2D array 165 | array = np.arange(400).reshape(20, 20) 166 | x = np.linspace(0, 19, 100) 167 | y = np.linspace(0, 19, 100) 168 | xx, yy = np.meshgrid(x, y) 169 | points = np.stack([yy, xx], axis=-1).reshape(-1, 2) 170 | values = sample_interpolated_bilinear(array, points) 171 | if visualize: 172 | import matplotlib.pyplot as plt 173 | 174 | plt.imshow(array) 175 | plt.show() 176 | print(points, values) 177 | 178 | plt.imshow(values.reshape(100, 100)) 179 | plt.show() 180 | -------------------------------------------------------------------------------- /terrain_generator/test/test_wfc.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | from ..wfc.wfc import WFCCore, ConnectionManager 6 | 7 | 8 | def test_get_neighbours(): 9 | 10 | # Test 2D 11 | connections = { 12 | 0: {(-1, 0): (0, 1), (0, -1): (0), (1, 0): (0, 1), (0, 1): (0, 1)}, # Mountain 13 | 1: {(-1, 0): (0, 1, 2), (0, -1): (0, 1), (1, 0): (0, 1, 2), (0, 1): (0, 1, 2)}, # Sand 14 | 2: {(-1, 0): (1, 2), (0, -1): (2), (1, 0): (2), (0, 1): (2)}, # Water 15 | } 16 | wfc = WFCCore(3, connections, (10, 10)) 17 | n, d = wfc._get_neighbours((2, 2)) 18 | assert n.dtype == int 19 | assert n.shape == (4, 2) 20 | assert sorted(n.tolist()) == sorted([[1, 2], [3, 2], [2, 1], [2, 3]]) 21 | 22 | # Test 3D 23 | directions = [(-1, 0, 0), (0, -1, 0), (0, 0, -1), (1, 0, 0), (0, 1, 0), (0, 0, 1)] 24 | connections = { 25 | 0: {directions[i]: (i) for i in range(6)}, 26 | 1: {directions[i]: (i) for i in range(6)}, 27 | 2: {directions[i]: (i) for i in range(6)}, 28 | } 29 | wfc = WFCCore(3, connections, (10, 10, 10), dimensions=3) 30 | n, d = wfc._get_neighbours((2, 2, 2)) 31 | assert n.dtype == int 32 | assert n.shape == (6, 3) 33 | assert sorted(n.tolist()) == sorted([[1, 2, 2], [3, 2, 2], [2, 1, 2], [2, 3, 2], [2, 2, 1], [2, 2, 3]]) 34 | 35 | 36 | def test_2d_case(visualize=False): 37 | connections = { 38 | 0: {(-1, 0): (0, 1), (0, -1): (0), (1, 0): (0, 1), (0, 1): (0, 1)}, # Mountain 39 | 1: {(-1, 0): (0, 1, 2), (0, -1): (0, 1), (1, 0): (0, 1, 2), (0, 1): (0, 1, 2)}, # Sand 40 | 2: {(-1, 0): (1, 2), (0, -1): (2), (1, 0): (2), (0, 1): (2)}, # Water 41 | } 42 | wfc = WFCCore(3, connections, (20, 20)) 43 | n, d = wfc._get_neighbours((9, 9)) 44 | wfc.init_randomly() 45 | wfc.solve() 46 | wave = wfc.wave.wave 47 | 48 | if visualize: 49 | print("Neighbours:", n, n.dtype, d, d.dtype) 50 | import matplotlib.pyplot as plt 51 | from matplotlib.colors import LinearSegmentedColormap 52 | 53 | # Define the colors and values for the custom colormap 54 | colors = [(1, 0, 0), (0, 1, 0), (0, 0, 1)] 55 | values = [0, 1, 2] 56 | 57 | # Create the custom colormap 58 | cmap = LinearSegmentedColormap.from_list("custom", colors, N=len(values)) 59 | 60 | # Use imshow to display the array with the custom colormap 61 | plt.imshow(wave, cmap=cmap) 62 | 63 | # Show the plot 64 | plt.show() 65 | 66 | 67 | def test_3d_case(visualize=False): 68 | directions = [(-1, 0, 0), (0, -1, 0), (0, 0, -1), (1, 0, 0), (0, 1, 0), (0, 0, 1)] 69 | connections = { 70 | 0: {directions[i]: (0, 1, 2) for i in range(6)}, 71 | 1: {directions[i]: (0, 1, 2) for i in range(6)}, 72 | 2: {directions[i]: (0, 2) for i in range(6)}, 73 | } 74 | wfc = WFCCore(3, connections, [10, 10, 3], dimensions=3) 75 | n, d = wfc._get_neighbours((9, 9, 1)) 76 | wfc.init_randomly() 77 | wfc.solve() 78 | wave = wfc.wave.wave 79 | if visualize: 80 | print("Neighbours:", n, n.dtype, d, d.dtype) 81 | print("wave ", wave) 82 | 83 | 84 | def test_connections(): 85 | # cm = ConnectionManager(load_from_cache=False) 86 | # cm.register_tile("tile_1", {"up": "abc", "down": "bbb", "left": "ccc", "right": "ddd"}) 87 | # cm.register_tile("tile_2", {"up": "abc", "down": "bbb", "left": "ddd", "right": "ccc"}) 88 | # cm.register_tile("tile_3", {"up": "bbb", "down": "cba", "left": "ccc", "right": "ddd"}) 89 | # cm.register_tile("tile_4", {"up": "bbb", "down": "cba", "left": "ddd", "right": "ccc"}) 90 | # 91 | # d = cm.get_connection_dict() 92 | # print("d = ", d) 93 | # # for k, v in d.items(): 94 | # # # print(k, v) 95 | # # for kk, vv in v.items(): 96 | # # print(kk, vv) 97 | # assert d == { 98 | # 0: {(-1, 0): {2, 3}, (1, 0): {2, 3}, (0, -1): {1, 3}, (0, 1): {1, 3}}, 99 | # 1: {(-1, 0): {2, 3}, (1, 0): {2, 3}, (0, -1): {0, 2}, (0, 1): {0, 2}}, 100 | # 2: {(-1, 0): {0, 1}, (1, 0): {0, 1}, (0, -1): {1, 3}, (0, 1): {1, 3}}, 101 | # 3: {(-1, 0): {0, 1}, (1, 0): {0, 1}, (0, -1): {0, 2}, (0, 1): {0, 2}}, 102 | # } 103 | 104 | cm = ConnectionManager(load_from_cache=False) 105 | cm.register_tile("tile_1", {"up": (0, 1, 2), "down": (1, 1, 1), "left": (2, 2, 2), "right": (3, 3, 3)}) 106 | cm.register_tile("tile_2", {"up": (0, 1, 2), "down": (1, 1, 1), "left": (3, 3, 3), "right": (2, 2, 2)}) 107 | cm.register_tile("tile_3", {"up": (1, 1, 1), "down": (2, 1, 0), "left": (2, 2, 2), "right": (3, 3, 3)}) 108 | cm.register_tile("tile_4", {"up": (1, 1, 1), "down": (2, 1, 0), "left": (3, 3, 3), "right": (2, 2, 2)}) 109 | 110 | d = cm.get_connection_dict() 111 | # print("d = ", d) 112 | # for k, v in d.items(): 113 | # # print(k, v) 114 | # for kk, vv in v.items(): 115 | # print(kk, vv) 116 | # assert d == { 117 | # 0: {(-1, 0): {2, 3}, (1, 0): {2, 3}, (0, -1): {1, 3}, (0, 1): {1, 3}}, 118 | # 1: {(-1, 0): {2, 3}, (1, 0): {2, 3}, (0, -1): {0, 2}, (0, 1): {0, 2}}, 119 | # 2: {(-1, 0): {0, 1}, (1, 0): {0, 1}, (0, -1): {1, 3}, (0, 1): {1, 3}}, 120 | # 3: {(-1, 0): {0, 1}, (1, 0): {0, 1}, (0, -1): {0, 2}, (0, 1): {0, 2}}, 121 | # } 122 | 123 | wfc = WFCCore( 124 | len(cm.names), 125 | d, 126 | [10, 10], 127 | dimensions=2, 128 | ) 129 | # print("Start solving...") 130 | # if len(init_tiles) > 0: 131 | # # print("init ", init_args) 132 | # for (name, index) in init_tiles: 133 | # tile_id = self.cm.names.index(name) 134 | # # print("idx ", idx) 135 | # wfc.init(index, tile_id) 136 | # else: 137 | wfc.init_randomly() 138 | wave = wfc.solve() 139 | # print("wave ", wave) 140 | 141 | # connections = { 142 | # 0: {(-1, 0): (0, 1), (0, -1): (0), (1, 0): (0, 1), (0, 1): (0, 1)}, # Mountain 143 | # 1: {(-1, 0): (0, 1, 2), (0, -1): (0, 1), (1, 0): (0, 1, 2), (0, 1): (0, 1, 2)}, # Sand 144 | # 2: {(-1, 0): (1, 2), (0, -1): (2), (1, 0): (2), (0, 1): (2)}, # Water 145 | # } 146 | 147 | # import matplotlib.pyplot as plt 148 | # from matplotlib.colors import LinearSegmentedColormap 149 | # 150 | # # Define the colors and values for the custom colormap 151 | # colors = [(1, 0, 0), (0, 1, 0), (0, 0, 1)] 152 | # values = [0, 1, 2] 153 | # 154 | # # Create the custom colormap 155 | # cmap = LinearSegmentedColormap.from_list("custom", colors, N=len(values)) 156 | # 157 | # # Use imshow to display the array with the custom colormap 158 | # plt.imshow(wave, cmap=cmap) 159 | # 160 | # # Show the plot 161 | # plt.show() 162 | -------------------------------------------------------------------------------- /terrain_generator/trimesh_tiles/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/terrain-generator/7be9d9a6ebbed7dc669486426184a4694bfd0bb6/terrain_generator/trimesh_tiles/__init__.py -------------------------------------------------------------------------------- /terrain_generator/trimesh_tiles/mesh_parts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/terrain-generator/7be9d9a6ebbed7dc669486426184a4694bfd0bb6/terrain_generator/trimesh_tiles/mesh_parts/__init__.py -------------------------------------------------------------------------------- /terrain_generator/trimesh_tiles/mesh_parts/basic_parts.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import trimesh 6 | import numpy as np 7 | from .mesh_parts_cfg import ( 8 | MeshPartsCfg, 9 | PlatformMeshPartsCfg, 10 | HeightMapMeshPartsCfg, 11 | WallPartsCfg, 12 | CapsuleMeshPartsCfg, 13 | BoxMeshPartsCfg, 14 | ) 15 | from ...utils import ( 16 | merge_meshes, 17 | merge_two_height_meshes, 18 | convert_heightfield_to_trimesh, 19 | merge_two_height_meshes, 20 | get_height_array_of_mesh, 21 | ENGINE, 22 | ) 23 | 24 | 25 | def create_floor(cfg: MeshPartsCfg, **kwargs): 26 | dims = [cfg.dim[0], cfg.dim[1], cfg.floor_thickness] 27 | pose = np.eye(4) 28 | pose[:3, -1] = [0, 0, -cfg.dim[2] / 2.0 + cfg.floor_thickness / 2.0 + cfg.height_offset] 29 | floor = trimesh.creation.box(dims, pose) 30 | return floor 31 | 32 | 33 | def create_standard_wall(cfg: WallPartsCfg, edge: str = "bottom", **kwargs): 34 | if edge == "bottom": 35 | dim = [cfg.dim[0], cfg.wall_thickness, cfg.wall_height] 36 | pos = [ 37 | 0, 38 | -cfg.dim[1] / 2.0 + cfg.wall_thickness / 2.0, 39 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 40 | ] 41 | elif edge == "up": 42 | dim = [cfg.dim[0], cfg.wall_thickness, cfg.wall_height] 43 | pos = [ 44 | 0, 45 | cfg.dim[1] / 2.0 - cfg.wall_thickness / 2.0, 46 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 47 | ] 48 | elif edge == "left": 49 | dim = [cfg.wall_thickness, cfg.dim[1], cfg.wall_height] 50 | pos = [ 51 | -cfg.dim[0] / 2.0 + cfg.wall_thickness / 2.0, 52 | 0, 53 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 54 | ] 55 | elif edge == "right": 56 | dim = [cfg.wall_thickness, cfg.dim[1], cfg.wall_height] 57 | pos = [ 58 | cfg.dim[0] / 2.0 - cfg.wall_thickness / 2.0, 59 | 0, 60 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 61 | ] 62 | elif edge == "middle_bottom": 63 | dim = [cfg.wall_thickness, cfg.dim[1] / 2.0 + cfg.wall_thickness / 2.0, cfg.wall_height] 64 | pos = [ 65 | 0, 66 | -cfg.dim[1] / 4.0 + cfg.wall_thickness / 4.0, 67 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 68 | ] 69 | elif edge == "middle_up": 70 | dim = [cfg.wall_thickness, cfg.dim[1] / 2.0 + cfg.wall_thickness / 2.0, cfg.wall_height] 71 | pos = [ 72 | 0, 73 | cfg.dim[1] / 4.0 - cfg.wall_thickness / 4.0, 74 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 75 | ] 76 | elif edge == "middle_left": 77 | dim = [cfg.dim[0] / 2.0 + cfg.wall_thickness / 2.0, cfg.wall_thickness, cfg.wall_height] 78 | pos = [ 79 | -cfg.dim[0] / 4.0 + cfg.wall_thickness / 4.0, 80 | 0, 81 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 82 | ] 83 | elif edge == "middle_right": 84 | dim = [cfg.dim[0] / 2.0 + cfg.wall_thickness / 2.0, cfg.wall_thickness, cfg.wall_height] 85 | pos = [ 86 | cfg.dim[0] / 4.0 - cfg.wall_thickness / 4.0, 87 | 0, 88 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 89 | ] 90 | elif edge == "bottom_left": 91 | dim = [cfg.dim[0] / 2.0, cfg.wall_thickness, cfg.wall_height] 92 | pos = [ 93 | -cfg.dim[0] / 4.0, # + cfg.wall_thickness / 2.0, 94 | -cfg.dim[1] / 2.0 + cfg.wall_thickness / 2.0, 95 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 96 | ] 97 | elif edge == "bottom_right": 98 | dim = [cfg.dim[0] / 2.0, cfg.wall_thickness, cfg.wall_height] 99 | pos = [ 100 | cfg.dim[0] / 4.0, # - cfg.wall_thickness / 2.0, 101 | -cfg.dim[1] / 2.0 + cfg.wall_thickness / 2.0, 102 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 103 | ] 104 | elif edge == "right_bottom": 105 | dim = [cfg.wall_thickness, cfg.dim[1] / 2.0, cfg.wall_height] 106 | pos = [ 107 | cfg.dim[0] / 2.0 - cfg.wall_thickness / 2.0, 108 | -cfg.dim[1] / 4.0, # + cfg.wall_thickness / 2.0, 109 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 110 | ] 111 | elif edge == "right_up": 112 | dim = [cfg.wall_thickness, cfg.dim[1] / 2.0, cfg.wall_height] 113 | pos = [ 114 | cfg.dim[0] / 2.0 - cfg.wall_thickness / 2.0, 115 | cfg.dim[1] / 4.0, # - cfg.wall_thickness / 2.0, 116 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 117 | ] 118 | else: 119 | raise ValueError(f"Edge {edge} is not defined.") 120 | 121 | pose = np.eye(4) 122 | pose[:3, -1] = pos 123 | wall = trimesh.creation.box(dim, pose) 124 | return wall 125 | 126 | 127 | def create_door(cfg: WallPartsCfg, door_direction: str = "up", **kwargs): 128 | if door_direction == "bottom" or door_direction == "up": 129 | dim = [cfg.door_width, 2.0, cfg.door_height] 130 | pos = [0, 0, -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.door_height / 2.0] 131 | elif door_direction == "left" or door_direction == "right": 132 | dim = [2.0, cfg.door_width, cfg.door_height] 133 | pos = [0, 0, -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.door_height / 2.0] 134 | elif door_direction == "middle_bottom": 135 | dim = [2.0, cfg.door_width, cfg.door_height] 136 | pos = [ 137 | 0, 138 | -cfg.dim[1] / 4.0 + cfg.wall_thickness / 4.0, 139 | -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.door_height / 2.0, 140 | ] 141 | elif door_direction == "middle_up": 142 | dim = [2.0, cfg.door_width, cfg.door_height] 143 | pos = [ 144 | 0, 145 | cfg.dim[1] / 4.0 - cfg.wall_thickness / 4.0, 146 | -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.wall_height / 2.0, 147 | ] 148 | elif door_direction == "middle_left": 149 | dim = [cfg.door_width, 2.0, cfg.door_height] 150 | pos = [ 151 | -cfg.dim[0] / 4.0 + cfg.wall_thickness / 4.0, 152 | 0, 153 | -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.wall_height / 2.0, 154 | ] 155 | elif door_direction == "middle_right": 156 | dim = [cfg.door_width, 2.0, cfg.door_height] 157 | pos = [ 158 | cfg.dim[0] / 4.0 - cfg.wall_thickness / 4.0, 159 | 0, 160 | -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.wall_height / 2.0, 161 | ] 162 | else: 163 | return trimesh.Trimesh() 164 | 165 | pose = np.eye(4) 166 | pose[:3, -1] = pos 167 | door = trimesh.creation.box(dim, pose) 168 | return door 169 | 170 | 171 | def create_wall_mesh(cfg: WallPartsCfg, **kwargs): 172 | # Create the vertices of the wall 173 | floor = create_floor(cfg) 174 | meshes = [floor] 175 | # mesh = floor 176 | for wall_edges in cfg.wall_edges: 177 | wall = create_standard_wall(cfg, wall_edges) 178 | # wall = get_wall_with_door(cfg, wall_edges) 179 | meshes.append(wall) 180 | # mesh = merge_meshes([mesh, wall], cfg.minimal_triangles) 181 | mesh = merge_meshes(meshes, cfg.minimal_triangles) 182 | if cfg.create_door: 183 | door = create_door(cfg, cfg.door_direction) 184 | mesh = trimesh.boolean.difference([mesh, door], engine=ENGINE) 185 | return mesh 186 | 187 | 188 | def create_platform_mesh(cfg: PlatformMeshPartsCfg, **kwargs): 189 | meshes = [] 190 | min_h = 0.0 191 | if cfg.add_floor: 192 | meshes.append(create_floor(cfg)) 193 | min_h = cfg.floor_thickness 194 | 195 | arrays = [cfg.array] 196 | z_dim_arrays = [cfg.z_dim_array] 197 | 198 | # Additional arrays 199 | if cfg.arrays is not None: 200 | arrays += cfg.arrays 201 | if cfg.z_dim_arrays is not None: 202 | z_dim_arrays += cfg.z_dim_arrays 203 | 204 | for array, z_dim_array in zip(arrays, z_dim_arrays): 205 | dim_xy = [cfg.dim[0] / array.shape[0], cfg.dim[1] / array.shape[1]] 206 | for y in range(array.shape[1]): 207 | for x in range(array.shape[0]): 208 | if array[y, x] > min_h: 209 | h = array[y, x] 210 | dim = [dim_xy[0], dim_xy[1], h] 211 | if cfg.use_z_dim_array: 212 | z = z_dim_array[y, x] 213 | if z > 0.0 and z < h: 214 | dim = np.array([dim_xy[0], dim_xy[1], z_dim_array[y, x]]) 215 | pos = np.array( 216 | [ 217 | x * dim[0] - cfg.dim[0] / 2.0 + dim[0] / 2.0, 218 | -y * dim[1] + cfg.dim[1] / 2.0 - dim[1] / 2.0, 219 | h - dim[2] / 2.0 - cfg.dim[2] / 2.0, 220 | ] 221 | ) 222 | box_mesh = trimesh.creation.box(dim, trimesh.transformations.translation_matrix(pos)) 223 | meshes.append(box_mesh) 224 | if cfg.wall is not None: 225 | wall_mesh = create_wall_mesh(cfg.wall) 226 | meshes.append(wall_mesh) 227 | # mesh = merge_meshes([mesh, wall_mesh], False) 228 | # mesh.fill_holes() 229 | mesh = merge_meshes(meshes, cfg.minimal_triangles) 230 | mesh.fill_holes() 231 | return mesh 232 | 233 | 234 | def create_from_height_map(cfg: HeightMapMeshPartsCfg, **kwargs): 235 | mesh = trimesh.Trimesh() 236 | height_map = cfg.height_map 237 | 238 | if cfg.fill_borders: 239 | mesh = create_floor(cfg) 240 | 241 | height_map = height_map.copy() - cfg.dim[2] / 2.0 242 | height_map_mesh = convert_heightfield_to_trimesh( 243 | height_map, 244 | cfg.horizontal_scale, 245 | cfg.vertical_scale, 246 | cfg.slope_threshold, 247 | ) 248 | bottom_height_map = height_map * 0.0 + cfg.floor_thickness - cfg.dim[2] / 2.0 249 | bottom_mesh = convert_heightfield_to_trimesh( 250 | bottom_height_map, 251 | cfg.horizontal_scale, 252 | cfg.vertical_scale, 253 | cfg.slope_threshold, 254 | ) 255 | height_map_mesh = merge_two_height_meshes(height_map_mesh, bottom_mesh) 256 | 257 | mesh = merge_meshes([mesh, height_map_mesh], False) 258 | if cfg.simplify: 259 | mesh = mesh.simplify_quadratic_decimation(cfg.target_num_faces) 260 | trimesh.repair.fix_normals(mesh) 261 | return mesh 262 | 263 | 264 | # def create_capsule_mesh(cfg: CapsuleMeshPartsCfg, **kwargs): 265 | # # Create the vertices of the wall 266 | # if cfg.add_floor: 267 | # floor = create_floor(cfg) 268 | # meshes = [floor] 269 | # else: 270 | # meshes = [] 271 | # for i in range(len(cfg.radii)): 272 | # capsule = trimesh.creation.capsule( 273 | # radius=cfg.radii[i], 274 | # height=cfg.heights[i], 275 | # # transform=cfg.transformations[i], 276 | # ) 277 | # t = cfg.transformations[i].copy() 278 | # t[2, 3] -= cfg.dim[2] / 2.0 279 | # capsule.apply_transform(t) 280 | # meshes.append(capsule) 281 | # mesh = merge_meshes(meshes, cfg.minimal_triangles) 282 | # return mesh 283 | 284 | 285 | def create_box_mesh(cfg: BoxMeshPartsCfg, **kwargs): 286 | print("create box mesh!!!!") 287 | if cfg.add_floor: 288 | print("create floor") 289 | floor = create_floor(cfg) 290 | meshes = [floor] 291 | else: 292 | print("not create floor") 293 | meshes = [] 294 | for i in range(len(cfg.box_dims)): 295 | t = cfg.transformations[i].copy() 296 | t[2, 3] -= cfg.dim[2] / 2.0 297 | box = trimesh.creation.box( 298 | cfg.box_dims[i], 299 | t, 300 | ) 301 | # box.apply_transform(t) 302 | meshes.append(box) 303 | mesh = merge_meshes(meshes, cfg.minimal_triangles) 304 | return mesh 305 | 306 | 307 | def create_random_mesh(cfg: CapsuleMeshPartsCfg, **kwargs): 308 | # Create the vertices of the wall 309 | if cfg.add_floor: 310 | floor = create_floor(cfg) 311 | meshes = [floor] 312 | else: 313 | meshes = [] 314 | for i in range(len(cfg.radii)): 315 | capsule = trimesh.creation.capsule( 316 | radius=cfg.radii[i], 317 | height=cfg.heights[i], 318 | # transform=cfg.transformations[i], 319 | ) 320 | t = cfg.transformations[i].copy() 321 | t[2, 3] -= cfg.dim[2] / 2.0 322 | capsule.apply_transform(t) 323 | meshes.append(capsule) 324 | mesh = merge_meshes(meshes, cfg.minimal_triangles) 325 | return mesh 326 | 327 | 328 | def create_capsule_mesh(cfg: CapsuleMeshPartsCfg, mesh: trimesh.Trimesh = None, **kwargs): 329 | # Create the vertices of the wall 330 | meshes = [] 331 | positions = [] 332 | for i in range(len(cfg.radii)): 333 | capsule = trimesh.creation.capsule( 334 | radius=cfg.radii[i], 335 | height=cfg.heights[i], 336 | # transform=cfg.transformations[i], 337 | ) 338 | t = cfg.transformations[i].copy() 339 | t[2, 3] -= cfg.dim[2] / 2.0 340 | positions.append(t[0:3, 3]) 341 | capsule.apply_transform(t) 342 | meshes.append(capsule) 343 | 344 | # Get heights of each position of capsule 345 | if mesh is not None: 346 | positions = np.array(positions) 347 | x = positions[:, 0] 348 | y = positions[:, 1] 349 | origins = np.stack([x, y, np.ones_like(x) * cfg.dim[2] * 2], axis=-1) 350 | # print("origins ", origins) 351 | vectors = np.stack([np.zeros_like(x), np.zeros_like(y), -np.ones_like(x)], axis=-1) 352 | # print("vectors ", vectors) 353 | # # do the actual ray- mesh queries 354 | points, index_ray, index_tri = mesh.ray.intersects_location(origins, vectors, multiple_hits=True) 355 | # print("points ", points) 356 | # print("index_ray ", index_ray) 357 | # print("index_tri ", index_tri) 358 | translations = [] 359 | for idx in index_ray: 360 | # positions[idx, 2] += points[idx, 2] 361 | translations.append(cfg.dim[2] / 2.0 + points[idx, 2]) 362 | # print("translations ", translations) 363 | for i, m in enumerate(meshes): 364 | m.apply_translation([0, 0, translations[i]]) 365 | mesh = merge_meshes(meshes, cfg.minimal_triangles) 366 | return mesh 367 | 368 | 369 | if __name__ == "__main__": 370 | 371 | positions = [np.array([0.0, 0.0, 0.0]), np.array([0.0, 1.0, 0.0]), np.array([1.0, 0.0, 0.0])] 372 | transformations = [trimesh.transformations.random_rotation_matrix() for i in range(len(positions))] 373 | for i in range(len(positions)): 374 | transformations[i][:3, -1] = positions[i] 375 | capsule_cfg = CapsuleMeshPartsCfg( 376 | radii=(0.1, 0.2, 0.3), heights=(0.4, 0.5, 0.6), transformations=tuple(transformations) 377 | ) 378 | capsule_mesh = create_capsule_mesh(capsule_cfg) 379 | capsule_mesh.show() 380 | print(get_height_array_of_mesh(capsule_mesh, capsule_cfg.dim, 5)) 381 | 382 | cfg = HeightMapMeshPartsCfg(height_map=np.ones((3, 3)) * 1.4, target_num_faces=50) 383 | mesh = create_from_height_map(cfg) 384 | print(get_height_array_of_mesh(mesh, cfg.dim, 5)) 385 | mesh.show() 386 | 387 | # print("showed 1st ex") 388 | 389 | x = np.linspace(0, 1, 10) 390 | y = np.linspace(0, 1, 10) 391 | X, Y = np.meshgrid(x, y) 392 | # Z = np.sin(X * 2 * np.pi) * np.cos(Y * 2 * np.pi) 393 | Z = np.sin(X * 2 * np.pi) 394 | Z = (Z + 1.0) * 0.2 + 0.2 395 | cfg = HeightMapMeshPartsCfg(height_map=Z, target_num_faces=3000, simplify=True) 396 | mesh = create_from_height_map(cfg) 397 | print(get_height_array_of_mesh(mesh, cfg.dim, 5)) 398 | print("mesh faces ", mesh.faces.shape) 399 | mesh.show() 400 | 401 | cfg = PlatformMeshPartsCfg( 402 | array=np.array([[1, 0], [0, 0]]), 403 | z_dim_array=np.array([[0.5, 0], [0, 0]]), 404 | use_z_dim_array=True, 405 | ) 406 | mesh = create_platform_mesh(cfg) 407 | print(get_height_array_of_mesh(mesh, cfg.dim, 5)) 408 | mesh.show() 409 | 410 | cfg = PlatformMeshPartsCfg( 411 | name="platform_T", 412 | array=np.array([[1, 1, 1, 1, 1], [0, 1, 1, 1, 0], [0, 0, 1, 0, 0], [0, 0, 1, 0, 0], [0, 0, 1, 0, 0]]), 413 | rotations=(90, 180, 270), 414 | flips=(), 415 | weight=0.1, 416 | ) 417 | mesh = create_platform_mesh(cfg) 418 | print(get_height_array_of_mesh(mesh, cfg.dim, 5)) 419 | mesh.show() 420 | 421 | cfg = PlatformMeshPartsCfg(array=np.array([[1, 0], [0, 0]])) 422 | mesh = create_platform_mesh(cfg) 423 | print(get_height_array_of_mesh(mesh, cfg.dim, 5)) 424 | mesh.show() 425 | cfg = PlatformMeshPartsCfg(array=np.array([[2, 2], [0, 0]])) 426 | mesh = create_platform_mesh(cfg) 427 | print(get_height_array_of_mesh(mesh, cfg.dim, 5)) 428 | mesh.show() 429 | -------------------------------------------------------------------------------- /terrain_generator/trimesh_tiles/mesh_parts/create_tiles.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | from typing import Tuple, Callable 7 | import trimesh 8 | import functools 9 | 10 | from .indoor_parts import create_stairs_mesh 11 | from .basic_parts import ( 12 | create_floor, 13 | create_platform_mesh, 14 | create_from_height_map, 15 | create_wall_mesh, 16 | create_capsule_mesh, 17 | create_box_mesh, 18 | ) 19 | from .overhanging_parts import generate_wall_from_array, create_overhanging_boxes, create_floating_boxes 20 | from .mesh_parts_cfg import ( 21 | MeshPartsCfg, 22 | WallPartsCfg, 23 | MeshPattern, 24 | StairMeshPartsCfg, 25 | PlatformMeshPartsCfg, 26 | HeightMapMeshPartsCfg, 27 | CapsuleMeshPartsCfg, 28 | BoxMeshPartsCfg, 29 | CombinedMeshPartsCfg, 30 | WallMeshPartsCfg, 31 | OverhangingMeshPartsCfg, 32 | FloatingBoxesPartsCfg, 33 | ) 34 | from ...wfc.tiles import MeshTile 35 | from ...utils import get_height_array_of_mesh, get_cached_mesh_gen, merge_meshes 36 | 37 | # from alive_progress import alive_it 38 | 39 | 40 | def get_mesh_gen(cfg: MeshPartsCfg) -> Callable: 41 | if isinstance(cfg, WallPartsCfg): 42 | mesh_gen = create_wall_mesh 43 | elif isinstance(cfg, StairMeshPartsCfg): 44 | mesh_gen = create_stairs_mesh 45 | elif isinstance(cfg, PlatformMeshPartsCfg): 46 | mesh_gen = create_platform_mesh 47 | elif isinstance(cfg, HeightMapMeshPartsCfg): 48 | mesh_gen = create_from_height_map 49 | elif isinstance(cfg, CapsuleMeshPartsCfg): 50 | mesh_gen = create_capsule_mesh 51 | elif isinstance(cfg, BoxMeshPartsCfg): 52 | mesh_gen = create_box_mesh 53 | elif isinstance(cfg, WallMeshPartsCfg): 54 | mesh_gen = generate_wall_from_array 55 | elif isinstance(cfg, CombinedMeshPartsCfg): 56 | mesh_gens = [get_mesh_gen(c) for c in cfg.cfgs] 57 | 58 | def mesh_gen(cfg): 59 | mesh = trimesh.Trimesh() 60 | for i, gen in enumerate(mesh_gens): 61 | new_mesh = gen(cfg.cfgs[i], mesh=mesh) 62 | # mesh = merge_meshes([mesh, new_mesh], cfg.minimal_triangles) 63 | mesh = merge_meshes([mesh, new_mesh], False) 64 | return mesh 65 | 66 | # print("defined mesh_gen", mesh_gen) 67 | 68 | # mesh_gen = lambda: functools.reduce(lambda a, b: a + b, [gen() for gen in mesh_gens]) 69 | else: 70 | raise NotImplementedError(f"Mesh generator for {cfg} not implemented") 71 | return mesh_gen 72 | 73 | 74 | # @ray.remote 75 | def create_mesh_tile(cfg: MeshPartsCfg) -> MeshTile: 76 | mesh_gen = get_mesh_gen(cfg) 77 | cached_mesh_gen = get_cached_mesh_gen(mesh_gen, cfg, verbose=False, use_cache=cfg.load_from_cache) 78 | name = cfg.name 79 | mesh = cached_mesh_gen() 80 | # If edge array is not provided, create it from the mesh 81 | if cfg.edge_array is None: 82 | cfg.edge_array = get_height_array_of_mesh(mesh, cfg.dim, 5) 83 | # Create the tile 84 | if cfg.use_generator: 85 | return MeshTile(name, cached_mesh_gen, array=cfg.edge_array, mesh_dim=cfg.dim, weight=cfg.weight) 86 | else: 87 | return MeshTile(name, mesh, array=cfg.edge_array, mesh_dim=cfg.dim, weight=cfg.weight) 88 | 89 | 90 | def create_mesh_pattern(cfg: MeshPattern) -> dict: 91 | import ray 92 | 93 | ray.init(ignore_reinit_error=True) 94 | create_mesh_tile_remote = ray.remote(create_mesh_tile) 95 | 96 | tiles = [] 97 | print("Creating mesh pattern... ") 98 | for mesh_cfg in cfg.mesh_parts: 99 | tiles.append(create_mesh_tile_remote.remote(mesh_cfg)) 100 | print("Waiting for parallel creation... ") 101 | tiles = ray.get(tiles) 102 | all_tiles = [] 103 | for i, tile in enumerate(tiles): 104 | mesh_cfg = cfg.mesh_parts[i] 105 | all_tiles += tile.get_all_tiles(rotations=mesh_cfg.rotations, flips=mesh_cfg.flips) 106 | # tile = create_mesh_tile(mesh_cfg) 107 | # if tile is not None: 108 | # tiles += tile.get_all_tiles(rotations=mesh_cfg.rotations, flips=mesh_cfg.flips) 109 | tile_dict = {tile.name: tile for tile in all_tiles} 110 | return tile_dict 111 | -------------------------------------------------------------------------------- /terrain_generator/trimesh_tiles/mesh_parts/indoor_parts.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import trimesh 6 | import numpy as np 7 | from ...utils import ( 8 | merge_meshes, 9 | yaw_rotate_mesh, 10 | ENGINE, 11 | get_height_array_of_mesh, 12 | ) 13 | from .mesh_parts_cfg import ( 14 | WallPartsCfg, 15 | StairMeshPartsCfg, 16 | ) 17 | from .basic_parts import create_floor 18 | 19 | 20 | def create_standard_wall(cfg: WallPartsCfg, edge: str = "bottom"): 21 | if edge == "bottom": 22 | dim = [cfg.dim[0], cfg.wall_thickness, cfg.wall_height] 23 | pos = [ 24 | 0, 25 | -cfg.dim[1] / 2.0 + cfg.wall_thickness / 2.0, 26 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 27 | ] 28 | elif edge == "up": 29 | dim = [cfg.dim[0], cfg.wall_thickness, cfg.wall_height] 30 | pos = [ 31 | 0, 32 | cfg.dim[1] / 2.0 - cfg.wall_thickness / 2.0, 33 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 34 | ] 35 | elif edge == "left": 36 | dim = [cfg.wall_thickness, cfg.dim[1], cfg.wall_height] 37 | pos = [ 38 | -cfg.dim[0] / 2.0 + cfg.wall_thickness / 2.0, 39 | 0, 40 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 41 | ] 42 | elif edge == "right": 43 | dim = [cfg.wall_thickness, cfg.dim[1], cfg.wall_height] 44 | pos = [ 45 | cfg.dim[0] / 2.0 - cfg.wall_thickness / 2.0, 46 | 0, 47 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 48 | ] 49 | elif edge == "middle_bottom": 50 | dim = [cfg.wall_thickness, cfg.dim[1] / 2.0 + cfg.wall_thickness / 2.0, cfg.wall_height] 51 | pos = [ 52 | 0, 53 | -cfg.dim[1] / 4.0 + cfg.wall_thickness / 4.0, 54 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 55 | ] 56 | elif edge == "middle_up": 57 | dim = [cfg.wall_thickness, cfg.dim[1] / 2.0 + cfg.wall_thickness / 2.0, cfg.wall_height] 58 | pos = [ 59 | 0, 60 | cfg.dim[1] / 4.0 - cfg.wall_thickness / 4.0, 61 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 62 | ] 63 | elif edge == "middle_left": 64 | dim = [cfg.dim[0] / 2.0 + cfg.wall_thickness / 2.0, cfg.wall_thickness, cfg.wall_height] 65 | pos = [ 66 | -cfg.dim[0] / 4.0 + cfg.wall_thickness / 4.0, 67 | 0, 68 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 69 | ] 70 | elif edge == "middle_right": 71 | dim = [cfg.dim[0] / 2.0 + cfg.wall_thickness / 2.0, cfg.wall_thickness, cfg.wall_height] 72 | pos = [ 73 | cfg.dim[0] / 4.0 - cfg.wall_thickness / 4.0, 74 | 0, 75 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 76 | ] 77 | elif edge == "bottom_left": 78 | dim = [cfg.dim[0] / 2.0, cfg.wall_thickness, cfg.wall_height] 79 | pos = [ 80 | -cfg.dim[0] / 4.0, # + cfg.wall_thickness / 2.0, 81 | -cfg.dim[1] / 2.0 + cfg.wall_thickness / 2.0, 82 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 83 | ] 84 | elif edge == "bottom_right": 85 | dim = [cfg.dim[0] / 2.0, cfg.wall_thickness, cfg.wall_height] 86 | pos = [ 87 | cfg.dim[0] / 4.0, # - cfg.wall_thickness / 2.0, 88 | -cfg.dim[1] / 2.0 + cfg.wall_thickness / 2.0, 89 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 90 | ] 91 | elif edge == "right_bottom": 92 | dim = [cfg.wall_thickness, cfg.dim[1] / 2.0, cfg.wall_height] 93 | pos = [ 94 | cfg.dim[0] / 2.0 - cfg.wall_thickness / 2.0, 95 | -cfg.dim[1] / 4.0, # + cfg.wall_thickness / 2.0, 96 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 97 | ] 98 | elif edge == "right_up": 99 | dim = [cfg.wall_thickness, cfg.dim[1] / 2.0, cfg.wall_height] 100 | pos = [ 101 | cfg.dim[0] / 2.0 - cfg.wall_thickness / 2.0, 102 | cfg.dim[1] / 4.0, # - cfg.wall_thickness / 2.0, 103 | -cfg.dim[2] / 2.0 + cfg.wall_height / 2.0, 104 | ] 105 | else: 106 | raise ValueError(f"Edge {edge} is not defined.") 107 | 108 | pose = np.eye(4) 109 | pose[:3, -1] = pos 110 | wall = trimesh.creation.box(dim, pose) 111 | return wall 112 | 113 | 114 | def create_door(cfg: WallPartsCfg, door_direction: str = "up"): 115 | if door_direction == "bottom" or door_direction == "up": 116 | dim = [cfg.door_width, 2.0, cfg.door_height] 117 | pos = [0, 0, -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.door_height / 2.0] 118 | elif door_direction == "left" or door_direction == "right": 119 | dim = [2.0, cfg.door_width, cfg.door_height] 120 | pos = [0, 0, -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.door_height / 2.0] 121 | elif door_direction == "middle_bottom": 122 | dim = [2.0, cfg.door_width, cfg.door_height] 123 | pos = [ 124 | 0, 125 | -cfg.dim[1] / 4.0 + cfg.wall_thickness / 4.0, 126 | -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.door_height / 2.0, 127 | ] 128 | elif door_direction == "middle_up": 129 | dim = [2.0, cfg.door_width, cfg.door_height] 130 | pos = [ 131 | 0, 132 | cfg.dim[1] / 4.0 - cfg.wall_thickness / 4.0, 133 | -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.wall_height / 2.0, 134 | ] 135 | elif door_direction == "middle_left": 136 | dim = [cfg.door_width, 2.0, cfg.door_height] 137 | pos = [ 138 | -cfg.dim[0] / 4.0 + cfg.wall_thickness / 4.0, 139 | 0, 140 | -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.wall_height / 2.0, 141 | ] 142 | elif door_direction == "middle_right": 143 | dim = [cfg.door_width, 2.0, cfg.door_height] 144 | pos = [ 145 | cfg.dim[0] / 4.0 - cfg.wall_thickness / 4.0, 146 | 0, 147 | -cfg.dim[2] / 2.0 + cfg.floor_thickness + cfg.wall_height / 2.0, 148 | ] 149 | else: 150 | return trimesh.Trimesh() 151 | 152 | pose = np.eye(4) 153 | pose[:3, -1] = pos 154 | door = trimesh.creation.box(dim, pose) 155 | return door 156 | 157 | 158 | def create_wall_mesh(cfg: WallPartsCfg): 159 | # Create the vertices of the wall 160 | floor = create_floor(cfg) 161 | mesh = floor 162 | for wall_edges in cfg.wall_edges: 163 | wall = create_standard_wall(cfg, wall_edges) 164 | # wall = get_wall_with_door(cfg, wall_edges) 165 | mesh = merge_meshes([mesh, wall], cfg.minimal_triangles) 166 | if cfg.create_door: 167 | door = create_door(cfg, cfg.door_direction) 168 | mesh = trimesh.boolean.difference([mesh, door], engine=ENGINE) 169 | return mesh 170 | 171 | 172 | def create_standard_stairs(cfg: StairMeshPartsCfg.Stair): 173 | """Create a standard stair with a given number of steps and a given height. This will fill bottom.""" 174 | n_steps = cfg.n_steps 175 | step_height = cfg.total_height / n_steps 176 | step_depth = cfg.step_depth 177 | residual_depth = cfg.dim[1] - (n_steps + 1) * step_depth 178 | mesh = trimesh.Trimesh() 179 | stair_start_pos = np.array([0.0, -cfg.dim[1] / 2.0, -cfg.dim[2] / 2.0]) 180 | current_pos = stair_start_pos 181 | if cfg.add_residual_side_up is False: 182 | dims = np.array([cfg.step_width, residual_depth, cfg.height_offset]) 183 | current_pos += np.array([0.0, dims[1], 0.0]) 184 | if cfg.height_offset != 0.0: 185 | pos = current_pos + np.array([0.0, dims[1] / 2.0, dims[2] / 2.0]) 186 | step = trimesh.creation.box(dims, trimesh.transformations.translation_matrix(pos)) 187 | mesh = merge_meshes([mesh, step], cfg.minimal_triangles) 188 | for n in range(n_steps + 1): 189 | if n == 0: 190 | if cfg.height_offset > 0: 191 | dims = [cfg.step_width, cfg.step_depth, cfg.height_offset] 192 | else: 193 | dims = [cfg.step_width, cfg.step_depth, cfg.floor_thickness] 194 | else: 195 | dims = [cfg.step_width, cfg.step_depth, n * step_height + cfg.height_offset] 196 | pos = current_pos + np.array([0, dims[1] / 2.0, dims[2] / 2.0]) 197 | step = trimesh.creation.box(dims, trimesh.transformations.translation_matrix(pos)) 198 | current_pos += np.array([0.0, dims[1], 0.0]) 199 | mesh = merge_meshes([mesh, step], cfg.minimal_triangles) 200 | if cfg.add_residual_side_up is True: 201 | dims = np.array([cfg.step_width, residual_depth, n_steps * step_height + cfg.height_offset]) 202 | pos = current_pos + np.array([0.0, dims[1] / 2.0, dims[2] / 2.0]) 203 | step = trimesh.creation.box(dims, trimesh.transformations.translation_matrix(pos)) 204 | mesh = merge_meshes([mesh, step], cfg.minimal_triangles) 205 | return mesh 206 | 207 | 208 | def create_stairs(cfg: StairMeshPartsCfg.Stair): 209 | if cfg.stair_type == "standard": 210 | mesh = create_standard_stairs(cfg) 211 | 212 | dim = np.array([cfg.step_width, cfg.dim[1], cfg.total_height]) 213 | if cfg.direction == "front": 214 | mesh = mesh 215 | elif cfg.direction == "left": 216 | mesh = yaw_rotate_mesh(mesh, 90) 217 | dim = dim[np.array([1, 0, 2])] 218 | elif cfg.direction == "back": 219 | mesh = yaw_rotate_mesh(mesh, 180) 220 | elif cfg.direction == "right": 221 | mesh = yaw_rotate_mesh(mesh, 270) 222 | dim = dim[np.array([1, 0, 2])] 223 | 224 | if "left" in cfg.attach_side: 225 | mesh.apply_translation([-cfg.dim[0] / 2.0 + dim[0] / 2.0, 0, 0]) 226 | if "right" in cfg.attach_side: 227 | mesh.apply_translation([cfg.dim[0] / 2.0 - dim[0] / 2.0, 0, 0]) 228 | if "front" in cfg.attach_side: 229 | mesh.apply_translation([0, cfg.dim[1] / 2.0 - dim[1] / 2.0, 0]) 230 | if "back" in cfg.attach_side: 231 | mesh.apply_translation([0, -cfg.dim[1] / 2.0 + dim[1] / 2.0, 0]) 232 | return mesh 233 | 234 | 235 | def create_stairs_mesh(cfg: StairMeshPartsCfg): 236 | mesh = create_floor(cfg) 237 | for stair in cfg.stairs: 238 | stairs = create_stairs(stair) 239 | mesh = merge_meshes([mesh, stairs], cfg.minimal_triangles) 240 | if cfg.wall is not None: 241 | for wall_edges in cfg.wall.wall_edges: 242 | wall = create_standard_wall(cfg.wall, wall_edges) 243 | mesh = merge_meshes([mesh, wall], cfg.minimal_triangles) 244 | 245 | return mesh 246 | 247 | 248 | if __name__ == "__main__": 249 | 250 | # cfg = WallMeshPartsCfg(wall_edges=("left", )) 251 | # mesh = create_wall_mesh(cfg) 252 | # mesh.show() 253 | # # 254 | # cfg = WallMeshPartsCfg(wall_edges=("up", )) 255 | # mesh = create_wall_mesh(cfg) 256 | # mesh.show() 257 | # 258 | # cfg = WallMeshPartsCfg(wall_edges=("right", "bottom")) 259 | # mesh = create_wall_mesh(cfg) 260 | # mesh.show() 261 | 262 | # cfg = WallMeshPartsCfg(wall_edges=("bottom_right", "right_bottom")) 263 | # mesh = create_wall_mesh(cfg) 264 | # mesh.show() 265 | # 266 | # for i in range(10): 267 | # cfg = WallMeshPartsCfg(wall_edges=("middle_right", "middle_left"), door_direction="up") 268 | # mesh = create_wall_mesh(cfg) 269 | # mesh.show() 270 | 271 | # cfg = StairMeshPartsCfg() 272 | # mesh = create_stairs(cfg) 273 | # mesh.show() 274 | # 275 | # cfg = StairMeshPartsCfg() 276 | # mesh = create_stairs(cfg.stairs[0]) 277 | # mesh.show() 278 | 279 | # stair_straight = StairMeshPartsCfg( 280 | # name="stair_s", 281 | # rotations=(90, 180, 270), 282 | # flips=(), 283 | # weight=0.1, 284 | # stairs=( 285 | # StairMeshPartsCfg.Stair( 286 | # step_width=2.0, 287 | # # step_height=0.15, 288 | # step_depth=0.3, 289 | # total_height=1.0, 290 | # stair_type="standard", 291 | # direction="up", 292 | # add_residual_side_up=True, 293 | # attach_side="front", 294 | # add_rail=False, 295 | # ), 296 | # ), 297 | # # wall=WallMeshPartsCfg( 298 | # # name="wall", 299 | # # wall_edges=("middle_left", "middle_right"), 300 | # # ) 301 | # ) 302 | stair_wide = StairMeshPartsCfg( 303 | name="stair_w", 304 | rotations=(90, 180, 270), 305 | flips=(), 306 | weight=0.1, 307 | stairs=( 308 | StairMeshPartsCfg.Stair( 309 | step_width=2.0, 310 | step_depth=0.3, 311 | total_height=1.0, 312 | stair_type="standard", 313 | direction="up", 314 | add_residual_side_up=False, 315 | attach_side="front", 316 | add_rail=False, 317 | ), 318 | ), 319 | ) 320 | # from mesh_parts.mesh_parts_cfg import StairPattern 321 | # pattern = StairPattern(name="stairs") 322 | mesh = create_stairs_mesh(stair_wide) 323 | mesh.show() 324 | print(get_height_array_of_mesh(mesh, stair_wide.dim, 5)) 325 | 326 | stair_straight = StairMeshPartsCfg( 327 | name="stair_s", 328 | rotations=(90, 180, 270), 329 | flips=(), 330 | weight=0.1, 331 | stairs=( 332 | StairMeshPartsCfg.Stair( 333 | step_width=1.0, 334 | # step_height=0.15, 335 | step_depth=0.3, 336 | total_height=1.0, 337 | height_offset=1.0, 338 | stair_type="standard", 339 | direction="up", 340 | add_residual_side_up=True, 341 | attach_side="front_right", 342 | add_rail=False, 343 | ), 344 | ), 345 | ) 346 | mesh = create_stairs_mesh(stair_straight) 347 | mesh.show() 348 | print(get_height_array_of_mesh(mesh, stair_straight.dim, 5)) 349 | # 350 | # stair_straight = StairMeshPartsCfg( 351 | # name="stair_s", 352 | # rotations=(90, 180, 270), 353 | # flips=(), 354 | # weight=0.1, 355 | # stairs=( 356 | # StairMeshPartsCfg.Stair( 357 | # step_width=1.0, 358 | # # step_height=0.15, 359 | # step_depth=0.3, 360 | # total_height=1.0, 361 | # stair_type="standard", 362 | # direction="up", 363 | # gap_direction="up", 364 | # add_residual_side_up=True, 365 | # height_offset=1.0, 366 | # attach_side="front_right", 367 | # add_rail=False, 368 | # fill_bottom=True, 369 | # ), 370 | # ), 371 | # ) 372 | # # from mesh_parts.mesh_parts_cfg import StairPattern 373 | # # pattern = StairPattern(name="stairs") 374 | # mesh = create_stairs_mesh(stair_straight) 375 | # mesh.show() 376 | -------------------------------------------------------------------------------- /terrain_generator/trimesh_tiles/mesh_parts/mesh_parts_cfg.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | import trimesh 7 | from typing import Tuple, Optional, Union, Literal 8 | from dataclasses import dataclass 9 | 10 | 11 | @dataclass 12 | class MeshPartsCfg: 13 | name: str = "mesh" 14 | dim: Tuple[float, float, float] = (2.0, 2.0, 2.0) # x, y, z 15 | floor_thickness: float = 0.1 16 | minimal_triangles: bool = True 17 | weight: float = 1.0 18 | rotations: Tuple[Literal[90, 180, 270], ...] = () # (90, 180, 270) 19 | flips: Tuple[Literal["x", "y"], ...] = () # ("x", "y") 20 | height_offset: float = 0.0 21 | edge_array: Optional[np.ndarray] = None # Array for edge definition. If None, height of the mesh is used. 22 | use_generator: bool = True 23 | load_from_cache: bool = True 24 | 25 | 26 | @dataclass 27 | class WallPartsCfg(MeshPartsCfg): 28 | wall_thickness: float = 0.4 29 | wall_height: float = 3.0 30 | wall_edges: Tuple[str, ...] = () # bottom, up, left, right, middle_left, middle_right, middle_up, middle_bottom 31 | wall_type: str = "wall" # wall, window, door 32 | # wall_type_probs: Tuple[float, ...] = (0.6, 0.2, 0.2) # wall, window, door 33 | create_door: bool = False 34 | door_width: float = 0.8 35 | door_height: float = 1.5 36 | door_direction: str = "" # left, right, up, down, none 37 | # wall_array: np.ndarray = np.zeros((3, 3)) 38 | 39 | 40 | @dataclass 41 | class StairMeshPartsCfg(MeshPartsCfg): 42 | @dataclass 43 | class Stair(MeshPartsCfg): 44 | step_width: float = 1.0 45 | step_depth: float = 0.3 46 | n_steps: int = 5 47 | total_height: float = 1.0 48 | height_offset: float = 0.0 49 | stair_type: str = "standard" # stair, open, ramp 50 | add_residual_side_up: bool = True # If false, add to bottom. 51 | add_rail: bool = False 52 | direction: str = "up" 53 | attach_side: str = "left" 54 | 55 | stairs: Tuple[Stair, ...] = (Stair(),) 56 | wall: Optional[WallPartsCfg] = None 57 | 58 | 59 | @dataclass 60 | class PlatformMeshPartsCfg(MeshPartsCfg): 61 | array: np.ndarray = np.zeros((2, 2)) 62 | z_dim_array: np.ndarray = np.zeros((2, 2)) 63 | arrays: Optional[Tuple[np.ndarray, ...]] = None # Additional arrays 64 | z_dim_arrays: Optional[Tuple[np.ndarray, ...]] = None # Additional arrays 65 | add_floor: bool = True 66 | use_z_dim_array: bool = False # If true, the box height is determined by the z_dim_array. 67 | wall: Optional[WallPartsCfg] = None # It will be used to create the walls. 68 | 69 | 70 | @dataclass 71 | class HeightMapMeshPartsCfg(MeshPartsCfg): 72 | height_map: np.ndarray = np.ones((10, 10)) 73 | add_floor: bool = True 74 | vertical_scale: float = 1.0 75 | slope_threshold: float = 4.0 76 | fill_borders: bool = True 77 | simplify: bool = True 78 | target_num_faces: int = 500 79 | 80 | def __post_init__(self): 81 | self.horizontal_scale = self.dim[0] / (self.height_map.shape[0]) 82 | 83 | 84 | @dataclass 85 | class OverhangingMeshPartsCfg(MeshPartsCfg): 86 | connection_array: np.ndarray = np.zeros((3, 3)) 87 | height_array: Optional[np.ndarray] = np.zeros((3, 3)) # Height array of the terrain. 88 | mesh: Optional[trimesh.Trimesh] = None # Mesh of the terrain. 89 | obstacle_type: str = "wall" # wall, window, door 90 | 91 | 92 | @dataclass 93 | class WallMeshPartsCfg(OverhangingMeshPartsCfg): 94 | wall_thickness: float = 0.4 95 | wall_height: float = 3.0 96 | create_door: bool = False 97 | door_width: float = 0.8 98 | door_height: float = 1.5 99 | 100 | 101 | @dataclass 102 | class OverhangingBoxesPartsCfg(OverhangingMeshPartsCfg): 103 | gap_mean: float = 0.8 104 | gap_std: float = 0.2 105 | box_height: float = 0.5 106 | box_grid_n: int = 6 107 | # box_prob: float = 1.0 108 | 109 | 110 | @dataclass 111 | class FloatingBoxesPartsCfg(OverhangingMeshPartsCfg): 112 | n_boxes: int = 5 113 | box_dim_min: Tuple[float, float, float] = (0.1, 0.1, 0.1) 114 | box_dim_max: Tuple[float, float, float] = (1.0, 1.0, 1.0) 115 | roll_pitch_range: Tuple[float, float] = (0.0, np.pi / 3) # in rad 116 | yaw_range: Tuple[float, float] = (0.0, 2 * np.pi) # in rad 117 | min_height: float = 0.5 118 | max_height: float = 1.0 119 | 120 | 121 | @dataclass 122 | class MeshPattern: 123 | # name: str 124 | dim: Tuple[float, float, float] = (2.0, 2.0, 2.0) # x, y, z 125 | mesh_parts: Tuple[MeshPartsCfg, ...] = (MeshPartsCfg(),) 126 | 127 | 128 | @dataclass 129 | class CapsuleMeshPartsCfg(MeshPartsCfg): 130 | add_floor: bool = True 131 | radii: Tuple[float, ...] = () 132 | heights: Tuple[float, ...] = () 133 | transformations: Tuple[np.ndarray, ...] = () 134 | 135 | 136 | @dataclass 137 | class CylinderMeshPartsCfg(MeshPartsCfg): 138 | add_floor: bool = True 139 | radii: Tuple[float, ...] = () 140 | heights: Tuple[float, ...] = () 141 | transformations: Tuple[np.ndarray, ...] = () 142 | 143 | 144 | @dataclass 145 | class BoxMeshPartsCfg(MeshPartsCfg): 146 | add_floor: bool = True 147 | box_dims: Tuple[Tuple[float, float, float], ...] = () 148 | transformations: Tuple[np.ndarray, ...] = () 149 | 150 | 151 | @dataclass 152 | class RandomMeshPartsCfg(MeshPartsCfg): 153 | add_floor: bool = True 154 | meshes: Tuple[Union[CapsuleMeshPartsCfg, BoxMeshPartsCfg], ...] = () 155 | 156 | 157 | @dataclass 158 | class CombinedMeshPartsCfg(MeshPartsCfg): 159 | add_floor: bool = True 160 | cfgs: Tuple[MeshPartsCfg, ...] = () 161 | -------------------------------------------------------------------------------- /terrain_generator/trimesh_tiles/mesh_parts/mountain.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import trimesh 6 | import numpy as np 7 | from typing import Tuple 8 | 9 | from ...utils import convert_heightfield_to_trimesh 10 | from .tree import add_trees_on_terrain 11 | from perlin_numpy import generate_perlin_noise_2d, generate_fractal_noise_2d 12 | 13 | 14 | def generate_perlin_terrain( 15 | base_shape: Tuple = (256, 256), 16 | base_res: Tuple = (2, 2), 17 | base_octaves: int = 2, 18 | base_fractal_weight: float = 0.2, 19 | noise_res: Tuple = (4, 4), 20 | noise_octaves: int = 5, 21 | base_scale: float = 2.0, 22 | noise_scale: float = 1.0, 23 | horizontal_scale: float = 1.0, 24 | vertical_scale: float = 10.0, 25 | ): 26 | # Generate fractal noise instead of Perlin noise 27 | base = generate_perlin_noise_2d(base_shape, base_res, tileable=(True, True)) 28 | base += generate_fractal_noise_2d(base_shape, base_res, base_octaves, tileable=(True, True)) * base_fractal_weight 29 | 30 | # Use different weights for the base and noise heightfields 31 | noise = generate_fractal_noise_2d(base_shape, noise_res, noise_octaves, tileable=(True, True)) 32 | 33 | terrain_height = base * base_scale + noise * noise_scale 34 | 35 | terrain_mesh = convert_heightfield_to_trimesh(terrain_height, horizontal_scale, vertical_scale) 36 | 37 | terrain_mesh.vertices[:, 2] = trimesh.smoothing.filter_humphrey(terrain_mesh).vertices[:, 2] 38 | 39 | return terrain_mesh 40 | 41 | 42 | if __name__ == "__main__": 43 | terrain = generate_perlin_terrain(horizontal_scale=0.2, vertical_scale=3.0) 44 | # terrain.show() 45 | tree_mesh = add_trees_on_terrain( 46 | terrain, num_trees=100, tree_scale_range=(0.15, 0.55), tree_deg_range=(-60, 60), tree_cylinder_sections=4 47 | ) 48 | mesh = terrain + tree_mesh 49 | # mesh.show() 50 | bbox = mesh.bounding_box.bounds 51 | # Get the center of the bounding box. 52 | center = np.mean(bbox, axis=0) 53 | center[2] = 0.0 54 | # Translate the mesh to the center of the bounding box. 55 | mesh = mesh.apply_translation(-center) 56 | mesh.export("mountain.obj") 57 | -------------------------------------------------------------------------------- /terrain_generator/trimesh_tiles/mesh_parts/overhanging_parts.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import trimesh 6 | from typing import Callable 7 | import numpy as np 8 | from ...utils import ( 9 | merge_meshes, 10 | yaw_rotate_mesh, 11 | rotate_mesh, 12 | ENGINE, 13 | get_height_array_of_mesh, 14 | get_heights_from_mesh, 15 | euler_angles_to_rotation_matrix, 16 | ) 17 | from .mesh_parts_cfg import ( 18 | OverhangingBoxesPartsCfg, 19 | WallMeshPartsCfg, 20 | OverhangingMeshPartsCfg, 21 | FloatingBoxesPartsCfg, 22 | PlatformMeshPartsCfg, 23 | BoxMeshPartsCfg, 24 | # StairMeshPartsCfg, 25 | ) 26 | 27 | 28 | def create_wall(width, height, depth): 29 | mesh = trimesh.creation.box([width, height, depth]) 30 | return mesh 31 | 32 | 33 | # TODO: finish this. 34 | def create_horizontal_bar(width, height, depth): 35 | # mesh = trimesh.creation.box([width, height, depth]) 36 | mesh = trimesh.creation.cylinder() 37 | return mesh 38 | 39 | 40 | def generate_wall_from_array(cfg: WallMeshPartsCfg) -> trimesh.Trimesh: 41 | """generate wall mesh from connection array. 42 | Args: connection_array (np.ndarray) shape=(3, 3) 43 | Return: trimesh.Trimesh 44 | """ 45 | assert cfg.connection_array.shape[0] == 3 and cfg.connection_array.shape[1] == 3 46 | # If all connection is 0, return empty mesh 47 | if cfg.connection_array.sum() == 0: 48 | return trimesh.Trimesh() 49 | 50 | grid_size = cfg.dim[0] / cfg.connection_array.shape[0] 51 | meshes = [] 52 | wall_fn = create_wall 53 | for y in range(cfg.connection_array.shape[1]): 54 | for x in range(cfg.connection_array.shape[0]): 55 | if cfg.connection_array[x, y] > 0: 56 | pos = np.array([x * grid_size, y * grid_size, 0]) 57 | pos[:2] += grid_size / 2.0 - cfg.dim[0] / 2.0 58 | pos[2] += cfg.wall_height / 2.0 - cfg.dim[2] / 2.0 59 | if np.abs(pos[0]) > 1.0e-4 and np.abs(pos[1]) < 1.0e-4: 60 | mesh = wall_fn(grid_size, cfg.wall_thickness, cfg.wall_height) 61 | mesh.apply_translation(pos) 62 | meshes.append(mesh) 63 | elif np.abs(pos[0]) < 1.0e-4 and np.abs(pos[1]) > 1.0e-4: 64 | mesh = wall_fn(cfg.wall_thickness, grid_size, cfg.wall_height) 65 | mesh.apply_translation(pos) 66 | meshes.append(mesh) 67 | elif np.abs(pos[0]) < 1.0e-4 and np.abs(pos[1]) < 1.0e-4: 68 | # Get the coordinates of the neighboring walls 69 | neighbors = [] 70 | for dx, dy in [(0, 1), (0, -1), (1, 0), (-1, 0)]: 71 | if ( 72 | 0 <= x + dx < cfg.connection_array.shape[0] 73 | and 0 <= y + dy < cfg.connection_array.shape[1] 74 | and cfg.connection_array[x + dx, y + dy] > 0 75 | ): 76 | neighbors.append((dx, dy)) 77 | if not neighbors: 78 | continue 79 | 80 | # Calculate the dimensions of the center wall 81 | # width = grid_size + cfg.wall_thickness 82 | # height = grid_size + cfg.wall_thickness 83 | for dx, dy in neighbors: 84 | width = grid_size / 2.0 85 | height = grid_size / 2.0 86 | depth = cfg.wall_height 87 | p = pos.copy() 88 | if dx == 1: 89 | width += cfg.wall_thickness / 2.0 90 | height = cfg.wall_thickness 91 | p[0] += -cfg.wall_thickness / 4.0 + grid_size / 4.0 92 | elif dx == -1: 93 | width += cfg.wall_thickness / 2.0 94 | height = cfg.wall_thickness 95 | p[0] -= -cfg.wall_thickness / 4.0 + grid_size / 4.0 96 | elif dy == 1: 97 | height += cfg.wall_thickness / 2.0 98 | width = cfg.wall_thickness 99 | p[1] += -cfg.wall_thickness / 4.0 + grid_size / 4.0 100 | elif dy == -1: 101 | height += cfg.wall_thickness / 2.0 102 | width = cfg.wall_thickness 103 | p[1] -= -cfg.wall_thickness / 4.0 + grid_size / 4.0 104 | else: 105 | continue 106 | 107 | mesh = wall_fn(width, height, depth) 108 | mesh.apply_translation(p) 109 | meshes.append(mesh) 110 | else: 111 | mesh = wall_fn(grid_size, grid_size, cfg.wall_height) 112 | meshes.append(mesh) 113 | mesh = merge_meshes(meshes, minimal_triangles=cfg.minimal_triangles, engine=ENGINE) 114 | mesh = yaw_rotate_mesh(mesh, 270) # This was required to match the connection array 115 | return mesh 116 | 117 | 118 | def create_overhanging_boxes(cfg: OverhangingBoxesPartsCfg, **kwargs): 119 | if cfg.mesh is not None: 120 | height_array = get_height_array_of_mesh(cfg.mesh, cfg.dim, cfg.box_grid_n) 121 | elif cfg.height_array is not None: 122 | height_array = cfg.height_array 123 | else: 124 | height_array = np.zeros((cfg.box_grid_n, cfg.box_grid_n)) 125 | array = np.random.normal(cfg.gap_mean, cfg.gap_std, size=height_array.shape) 126 | array += height_array 127 | z_dim_array = np.ones_like(array) * cfg.box_height 128 | floating_array = array + z_dim_array 129 | 130 | overhanging_cfg = PlatformMeshPartsCfg( 131 | name="floating_boxes", 132 | dim=cfg.dim, 133 | array=floating_array, 134 | z_dim_array=z_dim_array, 135 | rotations=(90, 180, 270), 136 | flips=("x", "y"), 137 | weight=0.1, 138 | minimal_triangles=False, 139 | add_floor=False, 140 | use_z_dim_array=True, 141 | ) 142 | return overhanging_cfg 143 | 144 | 145 | def create_floating_boxes(cfg: FloatingBoxesPartsCfg, **kwargs): 146 | if cfg.mesh is None: 147 | raise ValueError("mesh must be provided") 148 | # positions 149 | x = np.random.uniform(-cfg.dim[0] / 2.0, cfg.dim[0] / 2.0, size=(cfg.n_boxes,)) 150 | y = np.random.uniform(-cfg.dim[1] / 2.0, cfg.dim[1] / 2.0, size=(cfg.n_boxes,)) 151 | z = np.random.uniform(cfg.min_height, cfg.max_height, size=(cfg.n_boxes,)) 152 | terrain_heights = get_heights_from_mesh(cfg.mesh, np.stack([x, y], axis=1)) 153 | # print("z = ", z) 154 | # print("terrain_heights ", terrain_heights) 155 | z += terrain_heights 156 | z += cfg.dim[2] / 2.0 157 | positions = np.stack([x, y, z], axis=1) 158 | # rotations 159 | roll = np.random.uniform(cfg.roll_pitch_range[0], cfg.roll_pitch_range[1], size=(cfg.n_boxes,)) 160 | pitch = np.random.uniform(cfg.roll_pitch_range[0], cfg.roll_pitch_range[1], size=(cfg.n_boxes,)) 161 | yaw = np.random.uniform(cfg.yaw_range[0], cfg.yaw_range[1], size=(cfg.n_boxes,)) 162 | R = euler_angles_to_rotation_matrix(roll, pitch, yaw) 163 | transformations = np.eye(4, dtype=np.float32).reshape(1, 4, 4).repeat(cfg.n_boxes, axis=0) 164 | transformations[:, :3, :3] = R 165 | transformations[:, :3, 3] = positions 166 | 167 | # box dims 168 | box_x = np.random.uniform(cfg.box_dim_min[0], cfg.box_dim_max[0], size=(cfg.n_boxes,)) 169 | box_y = np.random.uniform(cfg.box_dim_min[1], cfg.box_dim_max[1], size=(cfg.n_boxes,)) 170 | box_z = np.random.uniform(cfg.box_dim_min[2], cfg.box_dim_max[2], size=(cfg.n_boxes,)) 171 | box_dims = np.stack([box_x, box_y, box_z], axis=1) 172 | # print(box_dims, box_dims.shape) 173 | # print(transformations, transformations.shape) 174 | box_cfg = BoxMeshPartsCfg(box_dims=tuple(box_dims), transformations=tuple(transformations)) 175 | return box_cfg 176 | 177 | 178 | def create_table_mesh(top_size=(1.0, 1.0, 0.05), leg_size=(0.05, 0.05, 0.5), leg_positions=None): 179 | if leg_positions is None: 180 | leg_positions = [ 181 | (-top_size[0] / 2.0 + leg_size[0] / 2.0, -top_size[1] / 2.0 + leg_size[1] / 2.0), 182 | (-top_size[0] / 2.0 + leg_size[0] / 2.0, top_size[1] / 2.0 - leg_size[1] / 2.0), 183 | (top_size[0] / 2.0 - leg_size[0] / 2.0, -top_size[1] / 2.0 + leg_size[1] / 2.0), 184 | (top_size[0] / 2.0 - leg_size[0] / 2.0, top_size[1] / 2.0 - leg_size[1] / 2.0), 185 | ] 186 | # leg_positions = [(-0.45, -0.45), (0.45, -0.45), (0.45, 0.45), (-0.45, 0.45)] 187 | 188 | table_top = trimesh.creation.box(extents=top_size) 189 | # table_top.visual.face_colors = [100, 100, 255, 150] 190 | 191 | table_legs = [] 192 | for pos in leg_positions: 193 | leg = trimesh.creation.box(extents=leg_size) 194 | # leg.visual.face_colors = [100, 100, 255, 50] 195 | leg.apply_translation((pos[0], pos[1], -leg_size[2] / 2)) 196 | table_legs.append(leg) 197 | 198 | table = trimesh.util.concatenate([table_top, *table_legs]) 199 | table = table.apply_translation((0, 0, leg_size[2] + top_size[2] / 2)) 200 | 201 | return table 202 | 203 | 204 | def create_archway_mesh( 205 | array=np.array([[0.0, 0.0, 0.0], [1.0, 1.0, 1.0], [0.0, 0.0, 0.0]]), 206 | width=2.0, 207 | depth=2.0, 208 | height=1.0, 209 | # thickness=0.2, 210 | radius=None, 211 | ): 212 | if radius is None: 213 | radius = height / 2 214 | 215 | mesh = trimesh.creation.box(extents=(width, depth, height)) 216 | if array[0, 1] > 0.0: 217 | cylinder = trimesh.creation.cylinder(radius=radius, height=width / 2.0 + radius / 2.0) 218 | cylinder = rotate_mesh(cylinder, 90, [1, 0, 0]) 219 | cylinder.apply_translation((0, -width / 4.0, -height / 2)) 220 | mesh = trimesh.boolean.difference([mesh, cylinder]) 221 | if array[-1, 1] > 0.0: 222 | cylinder = trimesh.creation.cylinder(radius=radius, height=width / 2.0 + 0.1 + radius / 2.0) 223 | cylinder = rotate_mesh(cylinder, 90, [1, 0, 0]) 224 | cylinder.apply_translation((0.0, width / 4.0, -height / 2)) 225 | mesh = trimesh.boolean.difference([mesh, cylinder]) 226 | if array[1, 0] > 0.0: 227 | cylinder = trimesh.creation.cylinder(radius=radius, height=width / 2.0 + 0.1 + radius / 2.0) 228 | cylinder = rotate_mesh(cylinder, 90, [1, 0, 0]) 229 | cylinder = rotate_mesh(cylinder, 90, [0, 0, 1]) 230 | cylinder.apply_translation((-width / 4.0, 0, -height / 2)) 231 | mesh = trimesh.boolean.difference([mesh, cylinder]) 232 | if array[1, -1] > 0.0: 233 | cylinder = trimesh.creation.cylinder(radius=radius, height=width / 2.0 + 0.1 + radius / 2.0) 234 | cylinder = rotate_mesh(cylinder, 90, [1, 0, 0]) 235 | cylinder = rotate_mesh(cylinder, 90, [0, 0, 1]) 236 | cylinder.apply_translation((width / 4.0, 0, -height / 2)) 237 | mesh = trimesh.boolean.difference([mesh, cylinder]) 238 | 239 | # archway = trimesh.boolean.difference([box, cylinder]) 240 | # archway.apply_translation((0, -thickness / 2, 0)) 241 | # archway.apply_translation((0, -thickness / 2, 0)) 242 | return mesh 243 | 244 | 245 | # def create_archway(radius=0.5, height=1.0, num_segments=10): 246 | # arch = trimesh.creation.cylinder(radius=radius, height=height, sections=num_segments) 247 | # arch.apply_scale([1, height / (2 * radius), height / (2 * radius)]) 248 | # arch.apply_translation([0, 0, height / 2]) 249 | # # arch.apply_rotation([0, 0, np.pi / 2], point=[0, 0, 0]) 250 | # return arch 251 | 252 | 253 | def create_irregular_overhang_mesh(vertices, height=0.5): 254 | irregular_overhang = trimesh.creation.convex_hull(vertices) 255 | irregular_overhang.apply_translation((0, 0, height)) 256 | return irregular_overhang 257 | 258 | 259 | def get_cfg_gen(cfg: OverhangingMeshPartsCfg) -> Callable: 260 | if isinstance(cfg, OverhangingBoxesPartsCfg): 261 | cfg_gen = create_overhanging_boxes 262 | elif isinstance(cfg, FloatingBoxesPartsCfg): 263 | cfg_gen = create_floating_boxes 264 | else: 265 | raise NotImplementedError 266 | return cfg_gen 267 | 268 | 269 | if __name__ == "__main__": 270 | # table = create_table_mesh() 271 | # table.show() 272 | 273 | # vertices = [(-0.5, 0, 0), (0.5, 0, 0), (0.2, 0.2, 0.2), (-0.2, 0.2, 0.2), (0, 0.4, 0.3)] 274 | # 275 | # irregular_overhang = create_irregular_overhang_mesh(vertices, height=0.5) 276 | # irregular_overhang.show() 277 | 278 | archway = create_archway_mesh(array=np.array([[0.0, 0.0, 0.0], [1.0, 1.0, 1.0], [0.0, 0.0, 0.0]])) 279 | archway.show() 280 | 281 | archway = create_archway_mesh(array=np.array([[0.0, 1.0, 0.0], [1.0, 1.0, 0.0], [0.0, 0.0, 0.0]])) 282 | archway.show() 283 | -------------------------------------------------------------------------------- /terrain_generator/trimesh_tiles/mesh_parts/rough_parts.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import trimesh 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | from perlin_numpy import generate_perlin_noise_2d, generate_fractal_noise_2d 9 | 10 | 11 | from ...utils import ( 12 | merge_meshes, 13 | yaw_rotate_mesh, 14 | ENGINE, 15 | get_height_array_of_mesh, 16 | ) 17 | from .mesh_parts_cfg import ( 18 | WallPartsCfg, 19 | # StairMeshPartsCfg, 20 | HeightMapMeshPartsCfg, 21 | ) 22 | from .basic_parts import create_floor 23 | 24 | 25 | def generate_perlin_tile_configs( 26 | name, 27 | dim, 28 | weight, 29 | height=0.5, 30 | offset=0.0, 31 | seed=0, 32 | shape=(128, 128), 33 | res=(4, 4), 34 | base_weight=0.3, 35 | fractal_weight=0.2, 36 | target_num_faces=4000, 37 | ): 38 | 39 | np.random.seed(seed) 40 | noise = generate_perlin_noise_2d(shape, res, tileable=(True, True)) * base_weight 41 | noise += generate_fractal_noise_2d(shape, res, 3, tileable=(True, True)) * fractal_weight 42 | # noise += offset 43 | xx = np.linspace(-1, 1, shape[0]) 44 | yy = np.linspace(-1, 1, shape[1]) 45 | x, y = np.meshgrid(xx, yy) 46 | base_edge_array = np.clip(1.0 - (x**6 + y**6), 0.0, 1.0) 47 | noise *= base_edge_array 48 | cfgs = [] 49 | 50 | def generate_cfgs(noise, name): 51 | # noise = noise * 0.1 + 0.1 52 | 53 | # edge_array = np.zeros(shape) 54 | # xx = np.linspace(-1, 1, shape[0]) 55 | # yy = np.linspace(-1, 1, shape[1]) 56 | # x, y = np.meshgrid(xx, yy) 57 | 58 | step_height = 1.0 / (shape[1] - 2) 59 | arrays = [] 60 | ramp_patterns = ["flat", "wide", "corner", "corner_flipped"] 61 | for ramp_pattern in ramp_patterns: 62 | base_edge_array = np.clip(1.0 - (x**8 + y**8), 0.0, 1.0) 63 | 64 | if ramp_pattern == "flat": 65 | # edge_array = base_edge_array 66 | # offset_array = np.zeros(shape) 67 | offset_array = np.ones(shape) * offset 68 | # n = shape[0] // 10 69 | n = 3 70 | if offset < 0.1: 71 | offset_array[n:-n, n:-n] += 0.1 72 | # if offset > 0.1: 73 | edge_array = np.zeros(shape) 74 | edge_array[n:-n, n:-n] = 1.0 75 | 76 | new_array = noise * edge_array + offset_array 77 | arrays.append(new_array) 78 | elif ramp_pattern == "mountain": 79 | edge_array = np.clip(1.0 - (x**2 + y**2), 0.0, 1.0) 80 | offset_array = edge_array * height * 0.5 81 | if offset > 0.1: 82 | edge_array = np.ones(shape) 83 | 84 | new_array = noise * edge_array + offset_array 85 | arrays.append(new_array) 86 | else: 87 | array_1 = np.zeros(shape) 88 | array_2 = np.zeros(shape) 89 | h_1 = 0.0 90 | h_2 = 1.0 91 | for s in range(shape[0]): 92 | if ramp_pattern == "wide": 93 | array_1[:, s] = h_1 94 | array_2[:, s] = h_2 95 | elif ramp_pattern == "corner": 96 | array_1[:s, s] = h_1 97 | array_2[:s, s] = h_2 98 | array_1[s, :s] = h_1 99 | array_2[s, :s] = h_2 100 | array_1[s, s] = h_1 101 | array_2[s, s] = h_2 102 | elif ramp_pattern == "corner_flipped": 103 | array_1[:s, s] = 1.0 - h_1 104 | array_2[:s, s] = 1.0 - h_2 105 | array_1[s, :s] = 1.0 - h_1 106 | array_2[s, :s] = 1.0 - h_2 107 | array_1[s, s] = 1.0 - h_1 108 | array_2[s, s] = 1.0 - h_2 109 | if s > 0: 110 | h_1 = min(h_1 + step_height, 1.0) 111 | h_2 = max(h_2 - step_height, 0.0) 112 | # edge_array = array_1 113 | 114 | # if offset > 0.1: 115 | # array_1 += offset 116 | # array_2 += offset 117 | # array_1 = np.ones(shape) 118 | # array_2 = np.ones(shape) 119 | 120 | offset_array = array_1 * height + offset 121 | new_array = noise * array_1 + offset_array 122 | arrays.append(new_array) 123 | 124 | offset_array = array_2 * height + offset 125 | new_array = noise * array_2 + offset_array 126 | arrays.append(new_array) 127 | 128 | weight_per_tile = weight / (len(ramp_patterns)) 129 | # print("weight_per_tile", weight_per_tile) 130 | cfgs = [] 131 | for i, array in enumerate(arrays): 132 | cfg = HeightMapMeshPartsCfg( 133 | name=f"{name}_{i}", 134 | dim=dim, 135 | height_map=array, 136 | rotations=(90, 180, 270), 137 | flips=("x", "y"), 138 | weight=weight_per_tile, 139 | slope_threshold=0.5, 140 | target_num_faces=target_num_faces, 141 | simplify=True, 142 | ) 143 | cfgs.append(cfg) 144 | return cfgs 145 | 146 | cfgs += generate_cfgs(noise, name) 147 | noise = np.rot90(noise, 1) 148 | cfgs += generate_cfgs(noise, name + "_rotated") 149 | return cfgs 150 | 151 | 152 | if __name__ == "__main__": 153 | from .create_tiles import create_mesh_tile 154 | 155 | print("test") 156 | # generate_perlin_tile_configs("perlin", 1.0) 157 | cfgs = [] 158 | cfgs += generate_perlin_tile_configs("perlin", [2, 2, 2], weight=1.0) 159 | cfgs += generate_perlin_tile_configs("perlin_0.5", [2, 2, 2], weight=1.0, offset=0.5, height=0.5) 160 | print(cfgs) 161 | for cfg in cfgs: 162 | tile = create_mesh_tile(cfg) 163 | mesh = tile.get_mesh() 164 | print(get_height_array_of_mesh(mesh, cfg.dim, 5)) 165 | mesh.show() 166 | -------------------------------------------------------------------------------- /terrain_generator/trimesh_tiles/mesh_parts/tree.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import trimesh 6 | import random 7 | import numpy as np 8 | from ...utils import get_heights_from_mesh 9 | from typing import Tuple 10 | from alive_progress import alive_bar 11 | 12 | 13 | class LSystem: 14 | def __init__(self, axiom, rules, angle_adjustment=22.5, num_branches=2): 15 | self.axiom = axiom 16 | self.rules = rules 17 | self.angle_adjustment = angle_adjustment 18 | self.num_branches = num_branches 19 | 20 | def generate(self, iterations): 21 | state = self.axiom 22 | for i in range(iterations): 23 | new_state = "" 24 | for c in state: 25 | if c in self.rules: 26 | rule = self.rules[c] 27 | if "{" in rule: 28 | # Choose random rotation directions and substitute them into the rule 29 | directions = random.sample(["+", "-", "^", "&", "\\", "/"], self.num_branches + 2) 30 | rule_args = [directions[0], directions[1]] + [ 31 | f"X{{{i}}}" for i in range(2, self.num_branches + 2) 32 | ] 33 | rule = rule.format(*rule_args) 34 | else: 35 | rule = rule.format(self.angle_adjustment) 36 | new_state += rule 37 | else: 38 | new_state += c 39 | state = new_state 40 | return state 41 | 42 | 43 | def generate_tree_mesh( 44 | num_branches: int = 2, iterations: int = 3, angle_adjustment: float = 22.5, cylinder_sections: int = 8 45 | ): 46 | """ 47 | Generate a tree mesh using an L-system. 48 | Args: 49 | num_branches: Number of branches to generate 50 | iterations: Number of iterations to run the L-system 51 | angle_adjustment: Angle adjustment for each iteration 52 | cylinder_sections: Number of sections to use for the tree cylinders 53 | Returns: 54 | tree_mesh: A trimesh.Trimesh object representing the tree mesh 55 | """ 56 | rules = { 57 | "F": "FF", 58 | "X": "F[{0}X]{1}" 59 | + "".join([f"[{0}+X{{{i}}}]" for i in range(2, 2 + num_branches - 2)]) 60 | + "".join([f"[{0}-X{{{i}}}]" for i in range(2 + num_branches - 2, 2 + 2 * (num_branches - 2))]), 61 | "+": "v[/&[+\\^-]{0}]{1}-[{0}]^/\\&[-v+]+v", 62 | "-": "v[+&[-^{1}/]+^{0}]-{0}\\&[+v-]v+", 63 | } 64 | rules = { 65 | "F": "FF", 66 | "X": "F[{0}X]{1}" 67 | + "".join([f"[{0}+X{{{i}}}]" for i in range(2, 2 + num_branches - 2)]) 68 | + "".join([f"[{0}-X{{{i}}}]" for i in range(2 + num_branches - 2, 2 + 2 * (num_branches - 2))]), 69 | "+": "v[/&[+\\^-]{0}]{1}-[{0}]^/\\&[-v+]+v", 70 | "-": "v[+&[-^{1}/]+^{0}]-{0}\\&[+v-]v+", 71 | } 72 | rules = { 73 | "F": "FF", 74 | "X": "F[+X]F[-X]{0}" + "".join([f"[{0}+X{{{i}}}][{0}-X{{{i}}}]" for i in range(2, 2 + num_branches - 2)]), 75 | "+": "+{0}", 76 | "-": "-{0}", 77 | } 78 | 79 | # Generate the L-system state 80 | lsys = LSystem("X", rules, angle_adjustment=angle_adjustment, num_branches=num_branches) 81 | state = lsys.generate(iterations) 82 | 83 | # Initialize the tree mesh 84 | tree = trimesh.Trimesh() 85 | 86 | # Set the initial position and orientation of the turtle 87 | position = np.array([0, 0, 0]) 88 | direction = np.array([0, 0, 1]) 89 | rot_mat = np.eye(4) 90 | stack = [] 91 | 92 | # Define the turtle movement parameters 93 | step_size = 0.1 94 | angle = 20.7 * (np.pi / 180.0) 95 | 96 | # Generate the tree mesh 97 | cylinder_list = [] 98 | for c in state: 99 | if c == "F": 100 | # Move the turtle forward and add a cylinder to the list 101 | endpoint = position + step_size * direction 102 | distance = np.linalg.norm(endpoint) 103 | radius = max(0.005, min(0.04 - 0.02 * distance, 0.03)) 104 | cylinder = trimesh.creation.cylinder(radius=radius, height=step_size, sections=cylinder_sections) 105 | center = (position + endpoint) / 2.0 106 | cylinder.apply_transform(rot_mat) 107 | cylinder.apply_translation(center) 108 | cylinder_list.append(cylinder) 109 | position = endpoint 110 | elif c == "+": 111 | # Rotate the turtle around the X axis 112 | rot_matrix = trimesh.transformations.rotation_matrix(angle, [1, 0, 0]) 113 | elif c == "-": 114 | # Rotate the turtle around the X axis 115 | rot_matrix = trimesh.transformations.rotation_matrix(-angle, [1, 0, 0]) 116 | elif c == "&": 117 | # Rotate the turtle around the Y axis 118 | rot_matrix = trimesh.transformations.rotation_matrix(angle, [0, 1, 0]) 119 | elif c == "^": 120 | # Rotate the turtle around the Y axis 121 | rot_matrix = trimesh.transformations.rotation_matrix(-angle, [0, 1, 0]) 122 | elif c == "\\": 123 | # Rotate the turtle around the Z axis 124 | rot_matrix = trimesh.transformations.rotation_matrix(angle, [0, 0, 1]) 125 | elif c == "/": 126 | # Rotate the turtle around the Z axis 127 | rot_matrix = trimesh.transformations.rotation_matrix(-angle, [0, 0, 1]) 128 | elif c == "[": 129 | # Push the turtle position and direction onto the stack 130 | stack.append((position, direction, rot_mat)) 131 | elif c == "]": 132 | # Pop the turtle position and direction from the stack 133 | position, direction, rot_mat = stack.pop() 134 | else: 135 | pass 136 | if c in ["+", "-", "&", "^", "\\", "/"]: 137 | # Rotate the turtle around the X axis 138 | direction = np.dot(rot_matrix[:3, :3], direction) 139 | direction /= np.linalg.norm(direction) 140 | rot_mat = np.dot(rot_matrix, rot_mat) 141 | 142 | tree = trimesh.util.concatenate(cylinder_list) 143 | # Smooth the tree mesh 144 | tree = tree.smoothed() 145 | 146 | # Scale the tree mesh to a realistic size 147 | scale_factor = 10.0 148 | tree.apply_scale(scale_factor) 149 | 150 | return tree 151 | 152 | 153 | def add_trees_on_terrain( 154 | terrain_mesh: trimesh.Trimesh, 155 | num_trees: int = 10, 156 | tree_scale_range: Tuple[float, float] = (0.5, 1.5), 157 | tree_deg_range: Tuple[float, float] = (-30.0, 30.0), 158 | tree_cylinder_sections: int = 6, 159 | ): 160 | """ 161 | Add trees to a terrain mesh. 162 | Args: 163 | terrain_mesh: A trimesh.Trimesh object representing the terrain mesh 164 | num_trees: Number of trees to add 165 | tree_scale_range: Range of tree scales to use 166 | tree_deg_range: Range of tree rotation angles to use 167 | tree_cylinder_sections: Number of sections to use for the tree cylinders 168 | Returns: 169 | A trimesh.Trimesh object representing the terrain mesh with trees 170 | """ 171 | 172 | # Generate a tree mesh using the provided function 173 | 174 | bbox = terrain_mesh.bounding_box.bounds 175 | tree_meshes = [] 176 | # apply random rotations and translations to the tree meshes 177 | positions = np.zeros((num_trees, 3)) 178 | positions[:, 0] = np.random.uniform(bbox[0][0], bbox[1][0], size=(num_trees,)) 179 | positions[:, 1] = np.random.uniform(bbox[0][1], bbox[1][1], size=(num_trees,)) 180 | positions[:, 2] = get_heights_from_mesh(terrain_mesh, positions[:, :2]) 181 | 182 | tree_rad_range = (tree_deg_range[0] * np.pi / 180.0, tree_deg_range[1] * np.pi / 180.0) 183 | 184 | with alive_bar(num_trees, dual_line=True, title="tree generation") as bar: 185 | for i in range(num_trees): 186 | num_branches = np.random.randint(2, 4) 187 | tree_mesh = generate_tree_mesh(num_branches=num_branches, cylinder_sections=tree_cylinder_sections) 188 | tree_mesh.apply_scale(np.random.uniform(*tree_scale_range)) 189 | pose = np.eye(4) 190 | pose[:3, 3] = positions[i] 191 | q = trimesh.transformations.quaternion_from_euler( 192 | np.random.uniform(tree_rad_range[0], tree_rad_range[1]), 193 | np.random.uniform(tree_rad_range[0], tree_rad_range[1]), 194 | np.random.uniform(0, 2 * np.pi), 195 | ) 196 | pose[:3, :3] = trimesh.transformations.quaternion_matrix(q)[:3, :3] 197 | tree_mesh.apply_transform(pose) 198 | tree_meshes.append(tree_mesh) 199 | bar() 200 | 201 | # Merge all the tree meshes into a single mesh 202 | tree_mesh = trimesh.util.concatenate(tree_meshes) 203 | 204 | return tree_mesh 205 | 206 | 207 | if __name__ == "__main__": 208 | # Generate the tree mesh and export it to an OBJ file 209 | tree_mesh = generate_tree_mesh(num_branches=4, cylinder_sections=6) 210 | tree_mesh.show() 211 | tree_mesh.export("tree.obj") 212 | -------------------------------------------------------------------------------- /terrain_generator/trimesh_tiles/patterns/overhanging_patterns.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | 7 | from ..mesh_parts.mesh_parts_cfg import ( 8 | WallMeshPartsCfg, 9 | ) 10 | 11 | 12 | def generate_walls(name, dim, wall_height=3.0, wall_thickness=0.4, weight=1.0, wall_weights=[20.0, 0.5, 0.5, 0.1]): 13 | arrays = [ 14 | np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]), 15 | np.array([[0.0, 0.0, 0.0], [1.0, 1.0, 1.0], [0.0, 0.0, 0.0]]), 16 | np.array([[0.0, 1.0, 0.0], [1.0, 1.0, 0.0], [0.0, 0.0, 0.0]]), 17 | np.array([[0.0, 1.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]]), 18 | ] 19 | prefix = ["empty", "straight", "corner", "left"] 20 | cfgs = [] 21 | for array, prefix, w in zip(arrays, prefix, wall_weights): 22 | if prefix == "empty": 23 | load_from_cache = False 24 | rotations = () 25 | else: 26 | load_from_cache = True 27 | rotations = (90, 180, 270) 28 | cfg = WallMeshPartsCfg( 29 | name=f"{name}_{prefix}", 30 | dim=dim, 31 | wall_height=wall_height, 32 | wall_thickness=wall_thickness, 33 | connection_array=array, 34 | rotations=rotations, 35 | flips=(), 36 | weight=w * weight, 37 | load_from_cache=load_from_cache, 38 | ) 39 | cfgs.append(cfg) 40 | return tuple(cfgs) 41 | -------------------------------------------------------------------------------- /terrain_generator/utils/__init__.py: -------------------------------------------------------------------------------- 1 | from .utils import * 2 | from .mesh_utils import * 3 | from .nav_utils import * 4 | -------------------------------------------------------------------------------- /terrain_generator/utils/nav_utils.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | import trimesh 7 | import networkx as nx 8 | import open3d as o3d 9 | from typing import Union, Tuple, Optional 10 | import matplotlib.pyplot as plt 11 | 12 | from scipy.sparse import csr_matrix 13 | from scipy.sparse.csgraph import shortest_path 14 | import cv2 15 | 16 | from .mesh_utils import get_heights_from_mesh 17 | 18 | 19 | def get_height_array_of_mesh_with_resolution(mesh, resolution=0.4, border_offset=0.0, return_points: bool = False): 20 | # Get the bounding box of the mesh. 21 | bbox = mesh.bounding_box.bounds 22 | # Get the minimum and maximum of the bounding box. 23 | b_min = np.min(bbox, axis=0) 24 | b_max = np.max(bbox, axis=0) 25 | center = (b_min + b_max) / 2 26 | 27 | dim = np.array([b_max[0] - b_min[0], b_max[1] - b_min[1], b_max[2] - b_min[2]]) 28 | 29 | n_points = int((b_max[0] - b_min[0]) / resolution) 30 | 31 | x = np.linspace(b_min[0] + border_offset, b_max[0] - border_offset, n_points) 32 | y = np.linspace(b_min[1] + border_offset, b_max[1] - border_offset, n_points) 33 | xv, yv = np.meshgrid(x, y) 34 | xv = xv.flatten() 35 | yv = yv.flatten() 36 | origins = np.stack([xv, yv, np.ones_like(xv) * dim[2] * 2], axis=-1) 37 | 38 | heights = get_heights_from_mesh(mesh, origins) 39 | array = heights.reshape(n_points, n_points) 40 | origins[:, 2] = heights 41 | if return_points: 42 | return array, center[:2], origins 43 | else: 44 | return array, center[:2] 45 | 46 | 47 | def calc_spawnable_locations_on_terrain( 48 | mesh: trimesh.Trimesh, 49 | # num_points=1000, 50 | filter_size=(5, 5), 51 | spawnable_threshold=0.1, 52 | border_offset=1.0, 53 | resolution=0.4, 54 | # n_points_per_tile=5, 55 | visualize=False, 56 | ): 57 | """ 58 | Create spawnable locations from a mesh. 59 | Args :param mesh: Mesh to create spawnable locations from. 60 | Returns: Spawnable locations. 61 | """ 62 | array, center, origins = get_height_array_of_mesh_with_resolution( 63 | mesh, resolution=resolution, border_offset=border_offset, return_points=True 64 | ) 65 | 66 | if visualize: 67 | plt.imshow(array) 68 | plt.colorbar() 69 | plt.show() 70 | 71 | flat_filter = np.ones(filter_size) * -1 72 | flat_filter[filter_size[0] // 2, filter_size[1] // 2] = flat_filter.shape[0] * flat_filter.shape[1] - 1 73 | 74 | # Filter the image using filter2D, which has inputs: (grayscale image, bit-depth, kernel) 75 | filtered_img = cv2.filter2D(array, -1, flat_filter) 76 | 77 | if visualize: 78 | plt.imshow(filtered_img) 79 | plt.colorbar() 80 | plt.show() 81 | 82 | # visualize thresholded image 83 | plt.imshow(filtered_img < spawnable_threshold) 84 | plt.colorbar() 85 | plt.show() 86 | 87 | spawnable_indices = np.argwhere(np.abs(filtered_img) <= spawnable_threshold) 88 | # spawnable_idx = np.random.choice(np.arange(len(spawnable_indices)), num_points) 89 | # spawnable_idx = spawnable_indices[spawnable_idx] 90 | # Make it sparse 91 | spawnable_idx = spawnable_indices 92 | spawnable_locations = origins.reshape((filtered_img.shape[0], filtered_img.shape[1], 3))[ 93 | spawnable_idx[:, 0], spawnable_idx[:, 1] 94 | ] 95 | spawnable_heights = array[spawnable_idx[:, 0], spawnable_idx[:, 1]] 96 | 97 | spawnable_locations[:, 2] = spawnable_heights 98 | 99 | return spawnable_locations 100 | 101 | 102 | def get_sdf_of_points(points, sdf_array, center, resolution): 103 | """Deplicated""" 104 | point = points 105 | point = point - center 106 | point = point / resolution 107 | point = point.round().astype(int) 108 | point += np.array(sdf_array.shape) // 2 109 | is_valid = np.logical_and( 110 | np.logical_and( 111 | np.logical_and(point[:, 0] >= 0, point[:, 0] < sdf_array.shape[0]), 112 | np.logical_and(point[:, 1] >= 0, point[:, 1] < sdf_array.shape[1]), 113 | ), 114 | np.logical_and(point[:, 2] >= 0, point[:, 2] < sdf_array.shape[2]), 115 | ) 116 | point[:, 0] = np.clip(point[:, 0], 0, sdf_array.shape[0] - 1) 117 | point[:, 1] = np.clip(point[:, 1], 0, sdf_array.shape[1] - 1) 118 | point[:, 2] = np.clip(point[:, 2], 0, sdf_array.shape[2] - 1) 119 | 120 | sdf = np.ones(point.shape[0]) * 1000.0 121 | sdf[is_valid] = sdf_array[point[is_valid, 0], point[is_valid, 1], point[is_valid, 2]] 122 | return sdf 123 | 124 | 125 | def filter_spawnable_locations_with_sdf( 126 | spawnable_locations: np.ndarray, 127 | sdf_array: np.ndarray, 128 | height_offset: float = 0.5, 129 | sdf_resolution: float = 0.1, 130 | sdf_threshold: float = 0.2, 131 | ): 132 | # get sdf values for spawnable locations 133 | spawnable_locations[:, 2] += height_offset 134 | sdf_values = get_sdf_of_points(spawnable_locations, sdf_array, np.array([0, 0, 0]), sdf_resolution) 135 | # filter spawnable locations 136 | spawnable_locations = spawnable_locations[sdf_values > sdf_threshold] 137 | return spawnable_locations 138 | 139 | 140 | def calc_spawnable_locations_with_sdf( 141 | terrain_mesh: trimesh.Trimesh, 142 | sdf_array: np.ndarray, 143 | visualize: bool = False, 144 | height_offset: float = 0.5, 145 | sdf_resolution: float = 0.1, 146 | sdf_threshold: float = 0.4, 147 | ): 148 | spawnable_locations = calc_spawnable_locations_on_terrain(terrain_mesh, visualize=visualize) 149 | spawnable_locations = filter_spawnable_locations_with_sdf( 150 | spawnable_locations, sdf_array, height_offset, sdf_resolution, sdf_threshold 151 | ) 152 | return spawnable_locations 153 | 154 | 155 | def locations_to_graph(positions): 156 | G = nx.Graph() 157 | 158 | # Add nodes to the graph with their positions 159 | for i, pos in enumerate(positions): 160 | G.add_node(i, pos=pos) 161 | 162 | # Compute the distance matrix 163 | distances = np.sqrt(((positions[:, np.newaxis] - positions) ** 2).sum(axis=2)) 164 | 165 | # Create a mask for edges below the threshold distance 166 | threshold = 0.5 167 | mask = distances < threshold 168 | 169 | # Create a list of edge tuples from the mask 170 | edges = np.transpose(np.where(mask)) 171 | 172 | # Add edges to the graph 173 | G.add_edges_from(edges) 174 | 175 | # Print the graph nodes and edges 176 | # print("Nodes:", G.nodes(data=True)) 177 | # print("Edges:", G.edges()) 178 | return G 179 | 180 | 181 | def visualize_mesh_and_graphs( 182 | mesh: trimesh.Trimesh, 183 | points: Union[nx.Graph, np.ndarray], 184 | color_values: Optional[np.ndarray] = None, 185 | goal_pos: Optional[np.ndarray] = None, 186 | ): 187 | 188 | if isinstance(points, nx.Graph): 189 | points = nx.get_node_attributes(points, "pos") 190 | points = np.array(list(points.values())) 191 | 192 | # print("points ", points, points.shape) 193 | 194 | # Create a point cloud where the points are the occupied SDF grid points 195 | pcd = o3d.geometry.PointCloud() 196 | pcd.points = o3d.utility.Vector3dVector(points) 197 | 198 | viewer = o3d.visualization.Visualizer() 199 | viewer.create_window() 200 | 201 | viewer.add_geometry(pcd) 202 | 203 | # Mesh 204 | o3d_mesh = mesh.as_open3d 205 | o3d_mesh.compute_vertex_normals() 206 | 207 | viewer.add_geometry(o3d_mesh) 208 | 209 | # Set the point cloud colors based on the color values 210 | if color_values is not None: 211 | cmap = plt.get_cmap("rainbow") 212 | vmin = color_values.min() 213 | vmax = color_values.max() 214 | norm = plt.Normalize(vmin=vmin, vmax=vmax) 215 | sdf_colors = cmap(norm(color_values.flatten()))[:, :3] 216 | pcd.colors = o3d.utility.Vector3dVector(sdf_colors) 217 | 218 | if goal_pos is not None: 219 | goal_pos = goal_pos.reshape(-1) 220 | goal_pos = np.array([goal_pos[0], goal_pos[1], -0.5]) 221 | line_points = [goal_pos, goal_pos + np.array([0, 0, 4])] 222 | lines = [[0, 1]] 223 | colors = [[1, 0, 0] for i in range(len(lines))] 224 | line_set = o3d.geometry.LineSet() 225 | line_set.points = o3d.utility.Vector3dVector(line_points) 226 | line_set.lines = o3d.utility.Vector2iVector(lines) 227 | line_set.colors = o3d.utility.Vector3dVector(colors) 228 | viewer.add_geometry(line_set) 229 | 230 | # voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud(pcd, voxel_size=0.05) 231 | # o3d.visualization.draw_geometries([voxel_grid]) 232 | 233 | # Visualize the point cloud and the mesh 234 | # viewer.add_geometry(voxel_grid) 235 | opt = viewer.get_render_option() 236 | opt.show_coordinate_frame = True 237 | viewer.run() 238 | viewer.destroy_window() 239 | # o3d.visualization.draw_geometries([voxel_grid, o3d_mesh]) 240 | 241 | 242 | def create_2d_graph_from_height_array( 243 | height_array: np.ndarray, 244 | graph_ratio: int = 4, 245 | height_threshold: float = 0.4, 246 | invalid_cost=1000.0, 247 | use_diagonal: bool = True, 248 | ): 249 | # height_array = get_height_array_of_mesh_with_resolution(mesh, resolution=height_array_resolution) 250 | 251 | # graph_height_ratio = graph_resolution / height_array_resolution 252 | graph_shape = (np.array(height_array.shape) // graph_ratio).astype(int) 253 | 254 | G = nx.grid_2d_graph(*graph_shape) 255 | # Add diagonal edges 256 | if use_diagonal: 257 | G.add_edges_from( 258 | [((x, y), (x + 1, y + 1)) for x in range(graph_shape[0] - 1) for y in range(graph_shape[1] - 1)] 259 | + [((x + 1, y), (x, y + 1)) for x in range(graph_shape[0] - 1) for y in range(graph_shape[1] - 1)] 260 | ) 261 | # Add cost map to edges 262 | for (u, v) in G.edges(): 263 | cost = height_map_cost(u, v, height_array, graph_ratio, height_threshold, invalid_cost) 264 | G[u][v]["weight"] = cost 265 | return G 266 | 267 | 268 | def distance_matrix_from_graph(graph: nx.Graph): 269 | # Compute adjacency matrix 270 | adj_mtx = nx.adjacency_matrix(graph) 271 | 272 | g_mat = csr_matrix(adj_mtx) 273 | 274 | # Compute shortest path distances using Dijkstra's algorithm 275 | dist_matrix, _ = shortest_path(csgraph=g_mat, directed=False, return_predecessors=True) 276 | return dist_matrix 277 | 278 | 279 | def compute_distance_matrix( 280 | mesh: trimesh.Trimesh, 281 | graph_ratio: int = 4, 282 | height_threshold: float = 0.4, 283 | invalid_cost: float = 1000.0, 284 | height_map_resolution: float = 0.1, 285 | ): 286 | height_array, center = get_height_array_of_mesh_with_resolution(mesh, resolution=height_map_resolution) 287 | G = create_2d_graph_from_height_array( 288 | height_array, graph_ratio=graph_ratio, invalid_cost=invalid_cost, height_threshold=height_threshold 289 | ) 290 | dist_matrix = distance_matrix_from_graph(G) 291 | shape = (np.array(height_array.shape) // graph_ratio).astype(int) 292 | return dist_matrix, shape, center 293 | 294 | 295 | def height_map_cost( 296 | u: Tuple[int, int], 297 | v: Tuple[int, int], 298 | height_array: np.ndarray, 299 | ratio: int, 300 | height_threshold: float = 0.4, 301 | invalid_cost: float = 1000.0, 302 | ): 303 | # sample heights between u and v in height array. 304 | # number of points is determined by ratio 305 | ratio = int(ratio) 306 | um = np.array(u) * ratio 307 | vm = np.array(v) * ratio 308 | idx = np.linspace(um, vm, num=ratio + 1).astype(int) 309 | heights = height_array[idx[:, 0], idx[:, 1]] 310 | diffs = np.abs(heights[1:] - heights[:-1]) 311 | distance = np.linalg.norm(vm - um) / ratio 312 | costs = distance + invalid_cost * (diffs > height_threshold) 313 | return costs.sum() 314 | 315 | 316 | def visualize_distance(height_array, distance_matrix, graph_ratio, goal_pos, height_array_resolution=0.1): 317 | 318 | distance_shape = (np.array(height_array.shape) // graph_ratio).astype(int) 319 | grid_x, grid_y = np.meshgrid(np.arange(distance_shape[0]), np.arange(distance_shape[1]), indexing="ij") 320 | grid_z = height_array[grid_x * graph_ratio, grid_y * graph_ratio] 321 | distance_points = np.stack( 322 | [ 323 | grid_x.flatten() * graph_ratio * height_array_resolution, 324 | grid_y.flatten() * graph_ratio * height_array_resolution, 325 | grid_z.flatten(), 326 | ], 327 | axis=1, 328 | ) 329 | goal_idx = goal_pos[0] * distance_shape[0] + goal_pos[1] 330 | distances = distance_matrix[goal_idx, :] 331 | # Create a point cloud where the points are the occupied SDF grid points 332 | pcd = o3d.geometry.PointCloud() 333 | pcd.points = o3d.utility.Vector3dVector(distance_points) 334 | 335 | # Set the point cloud colors based on the SDF values 336 | cmap = plt.get_cmap("rainbow") 337 | norm = plt.Normalize(vmin=0.0, vmax=150.0) 338 | distance_colors = cmap(norm(distances.flatten()))[:, :3] 339 | pcd.colors = o3d.utility.Vector3dVector(distance_colors) 340 | 341 | voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud(pcd, voxel_size=0.20) 342 | 343 | # Visualize the point cloud and the mesh 344 | viewer = o3d.visualization.Visualizer() 345 | viewer.create_window() 346 | viewer.add_geometry(voxel_grid) 347 | opt = viewer.get_render_option() 348 | opt.show_coordinate_frame = True 349 | viewer.run() 350 | viewer.destroy_window() 351 | -------------------------------------------------------------------------------- /terrain_generator/utils/utils.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import os 6 | import numpy as np 7 | import torch 8 | import torch.nn.functional as F 9 | import trimesh 10 | from typing import Callable, Any, Optional, Union, Tuple 11 | from dataclasses import asdict, is_dataclass 12 | from itertools import product 13 | import copy 14 | import json 15 | from scipy.spatial.transform import Rotation 16 | 17 | 18 | ENGINE = "blender" 19 | # Cache dir is in the above directory as this file. 20 | # CACHE_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "__cache__/mesh_cache") 21 | CACHE_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "__cache__") 22 | # print("CACHE_DIR", CACHE_DIR) 23 | # CACHE_DIR = 24 | # ENGINE = "scad" 25 | 26 | 27 | class NpEncoder(json.JSONEncoder): 28 | def default(self, obj): 29 | if isinstance(obj, np.integer): 30 | return int(obj) 31 | if isinstance(obj, np.floating): 32 | return float(obj) 33 | if isinstance(obj, np.ndarray): 34 | return obj.tolist() 35 | if isinstance(obj, trimesh.Trimesh): 36 | return None 37 | return json.JSONEncoder.default(self, obj) 38 | 39 | 40 | def cfg_to_hash(cfg, exclude_keys=["weight", "load_from_cache"]): 41 | """MD5 hash of a config.""" 42 | import hashlib 43 | 44 | def tuple_to_str(d): 45 | new_d = {} 46 | for k, v in d.items(): 47 | if isinstance(v, dict): 48 | v = tuple_to_str(v) 49 | if isinstance(k, tuple): 50 | new_d[str(k)] = v 51 | else: 52 | new_d[k] = v 53 | return new_d 54 | 55 | if isinstance(cfg, dict): 56 | cfg_dict = copy.deepcopy(cfg) 57 | elif is_dataclass(cfg): 58 | cfg_dict = asdict(cfg) 59 | else: 60 | raise ValueError("cfg must be a dict or dataclass.") 61 | for key in exclude_keys: 62 | cfg_dict.pop(key, None) 63 | cfg_dict = tuple_to_str(cfg_dict) 64 | encoded = json.dumps(cfg_dict, sort_keys=True, cls=NpEncoder).encode() 65 | dhash = hashlib.md5() 66 | # We need to sort arguments so {'a': 1, 'b': 2} is 67 | # the same as {'b': 2, 'a': 1} 68 | # encoded = json.dumps(cfg, sort_keys=True).encode() 69 | dhash.update(encoded) 70 | return dhash.hexdigest() 71 | 72 | 73 | def get_cached_mesh_gen( 74 | mesh_gen_fn: Callable[[Any], trimesh.Trimesh], 75 | cfg, 76 | verbose=False, 77 | use_cache=True, 78 | ) -> Callable[[], trimesh.Trimesh]: 79 | """Generate a mesh if there's no cache. If there's cache, load from cache.""" 80 | code = cfg_to_hash(cfg) 81 | mesh_cache_dir = os.path.join(CACHE_DIR, "mesh_cache") 82 | os.makedirs(mesh_cache_dir, exist_ok=True) 83 | if hasattr(cfg, "name"): 84 | name = cfg.name 85 | else: 86 | name = "" 87 | 88 | mesh_name = f"{name}_{code}.obj" 89 | 90 | def mesh_gen() -> trimesh.Trimesh: 91 | if os.path.exists(os.path.join(mesh_cache_dir, mesh_name)) and use_cache: 92 | if verbose: 93 | print(f"Loading mesh {name} from cache {mesh_name} ...") 94 | mesh = trimesh.load_mesh(os.path.join(mesh_cache_dir, mesh_name)) 95 | else: 96 | # if verbose: 97 | if use_cache: 98 | print(f"{name} does not exist in cache, creating {mesh_name} ...") 99 | mesh = mesh_gen_fn(cfg) 100 | mesh.export(os.path.join(mesh_cache_dir, mesh_name)) 101 | return mesh 102 | 103 | return mesh_gen 104 | 105 | 106 | def check_validity(shape: Tuple[int], indices: Union[np.ndarray, torch.Tensor]): 107 | """Check if indices are valid for a given shape.""" 108 | if isinstance(indices, np.ndarray): 109 | indices = torch.from_numpy(indices) 110 | if len(shape) == 2: 111 | is_valid = torch.logical_and( 112 | torch.logical_and(indices[:, 0] >= 0, indices[:, 0] < shape[0]), 113 | torch.logical_and(indices[:, 1] >= 0, indices[:, 1] < shape[1]), 114 | ) 115 | elif len(shape) == 3: 116 | is_valid = torch.logical_and( 117 | torch.logical_and( 118 | torch.logical_and(indices[:, 0] >= 0, indices[:, 0] < shape[0]), 119 | torch.logical_and(indices[:, 1] >= 0, indices[:, 1] < shape[1]), 120 | ), 121 | torch.logical_and(indices[:, 2] >= 0, indices[:, 2] < shape[2]), 122 | ) 123 | else: 124 | raise ValueError(f"Invalid shape. shape: {shape}. shape dimension must be 2 or 3.") 125 | return is_valid 126 | 127 | 128 | def sample_interpolated_bilinear( 129 | grid: Union[np.ndarray, torch.Tensor], 130 | indices: Union[np.ndarray, torch.Tensor], 131 | invalid_value: float = 0.0, 132 | round_decimals: int = 5, 133 | ): 134 | """Sample a grid at given indices. If the indices are not integers, interpolate. 135 | Args: 136 | grid: (np.ndarray or torch.Tensor) of shape (H, W) or (H, W, D). 137 | indices: (np.ndarray or torch.Tensor) of shape (N, 2) or (N, 3). 138 | invalid_value: (float) value to return if the indices are invalid. 139 | round_decimals: (int) number of decimals to round the indices to. 140 | Returns: 141 | (np.ndarray or torch.Tensor) of shape (N,). 142 | """ 143 | 144 | use_pytorch = isinstance(grid, torch.Tensor) 145 | 146 | if isinstance(grid, np.ndarray): 147 | grid = torch.from_numpy(grid) 148 | if isinstance(indices, np.ndarray): 149 | indices = torch.from_numpy(indices) 150 | 151 | indices = torch.round(indices, decimals=round_decimals) 152 | # convert the float indices to integer indices 153 | floor_indices = torch.floor(indices - 0.0).long() 154 | is_valid = check_validity(grid.shape, floor_indices) 155 | values = torch.zeros_like(indices[:, 0]) 156 | weights = torch.zeros_like(indices[:, 0]) 157 | for delta in product(*[[0, 1] for _ in range(floor_indices.shape[-1])]): 158 | delta = torch.tensor(delta, dtype=torch.long).to(floor_indices.device) 159 | # neighboring_indices.append(floor_indices[:, i]) 160 | idx = floor_indices.clone() 161 | idx += delta 162 | # Trilinear interpolation 163 | w = (1.0 - (indices - idx.float()).abs()).prod(dim=-1) 164 | # neighboring_indices.append(idx) 165 | valid = check_validity(grid.shape, idx) 166 | is_valid = torch.logical_or(is_valid, valid) 167 | v = torch.ones_like(w) * invalid_value 168 | v[valid] = grid[[idx[valid, i] for i in range(idx.shape[-1])]].to(v.dtype) 169 | values[valid] += w[valid] * v[valid] 170 | weights[valid] += w[valid] 171 | values /= weights + 1e-6 172 | values[~is_valid] = invalid_value 173 | if not use_pytorch: 174 | values = values.cpu().numpy() 175 | return values 176 | 177 | 178 | def sample_interpolated( 179 | grid: Union[np.ndarray, torch.Tensor], 180 | indices: Union[np.ndarray, torch.Tensor], 181 | padding_mode: str = "zeros", 182 | invalid_value: float = 0.0, 183 | no_grad: bool = True, 184 | ): 185 | """Sample a grid at given indices. If the indices are not integers, interpolate. 186 | Args: 187 | grid: (np.ndarray or torch.Tensor) of shape (B, C, H, W) or (B, C, H, W, D). 188 | indices: (np.ndarray or torch.Tensor) of shape (B, N, 2) or (B, N, 3). 189 | invalid_value: (float) value to return if the indices are invalid. 190 | round_decimals: (int) number of decimals to round the indices to. 191 | Returns: 192 | (np.ndarray or torch.Tensor) of shape (N,). 193 | """ 194 | 195 | use_pytorch = isinstance(grid, torch.Tensor) 196 | 197 | if isinstance(grid, np.ndarray): 198 | grid = torch.from_numpy(grid) 199 | if isinstance(indices, np.ndarray): 200 | indices = torch.from_numpy(indices) 201 | 202 | grid = grid.float() 203 | indices = indices.float() 204 | 205 | # project the indices to [-1, 1] 206 | grid_shape = grid.shape 207 | old_indices = indices.clone() 208 | if len(grid_shape) == 4: 209 | indices[..., 0] = old_indices[..., 1] / (grid_shape[-1] - 1) * 2 - 1 210 | indices[..., 1] = old_indices[..., 0] / (grid_shape[-2] - 1) * 2 - 1 211 | elif len(grid_shape) == 5: 212 | indices[..., 0] = old_indices[..., 2] / (grid_shape[-1] - 1) * 2 - 1 213 | indices[..., 1] = old_indices[..., 1] / (grid_shape[-2] - 1) * 2 - 1 214 | indices[..., 2] = old_indices[..., 0] / (grid_shape[-3] - 1) * 2 - 1 215 | 216 | if no_grad: 217 | with torch.no_grad(): 218 | values = F.grid_sample(grid, indices, mode="bilinear", padding_mode=padding_mode, align_corners=True) 219 | else: 220 | values = F.grid_sample(grid, indices, mode="bilinear", padding_mode=padding_mode, align_corners=True) 221 | if invalid_value != 0.0: 222 | values = torch.where(values == 0.0, torch.tensor(invalid_value), values) 223 | if not use_pytorch: 224 | values = values.cpu().numpy() 225 | return values 226 | 227 | 228 | def euler_angles_to_rotation_matrix(roll: np.ndarray, pitch: np.ndarray, yaw: np.ndarray): 229 | """Convert euler angles to rotation matrix. 230 | Args: 231 | roll: (float) rotation around x-axis. 232 | pitch: (float) rotation around y-axis. 233 | yaw: (float) rotation around z-axis. 234 | Returns: 235 | (np.ndarray) of shape (3, 3). 236 | """ 237 | roll = np.expand_dims(roll, axis=-1) 238 | pitch = np.expand_dims(pitch, axis=-1) 239 | yaw = np.expand_dims(yaw, axis=-1) 240 | r = Rotation.from_euler("xyz", np.concatenate([roll, pitch, yaw], axis=-1), degrees=False) 241 | return r.as_matrix() 242 | -------------------------------------------------------------------------------- /terrain_generator/wfc/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/terrain-generator/7be9d9a6ebbed7dc669486426184a4694bfd0bb6/terrain_generator/wfc/__init__.py -------------------------------------------------------------------------------- /terrain_generator/wfc/tiles.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2023, Takahiro Miki. All rights reserved. 3 | # Licensed under the MIT license. See LICENSE file in the project root for details. 4 | # 5 | import numpy as np 6 | import trimesh 7 | import functools 8 | from typing import Dict, Optional, Any, Callable, Tuple, Union 9 | 10 | from .wfc import Direction2D, Direction3D 11 | from ..utils import flip_mesh, yaw_rotate_mesh, get_height_array_of_mesh 12 | 13 | 14 | class Tile: 15 | """Class to manage the tiles.""" 16 | 17 | def __init__(self, name: str, edges: Dict[str, str], dimension: int = 2, weight: float = 1.0): 18 | """Tile definition for the WFC algorithm. 19 | Args: 20 | name (str): Name of the tile. 21 | edges (Dict[str, str]): Dictionary of the edges of the tile. The keys are the directions and the values are the name of the edge. 22 | Example: 23 | tile = Tile(name="tile1", edges={"up": "edge1", "right": "edge2", "down": "edge3", "left": "edge4"}) 24 | """ 25 | self.name = name 26 | self.dimension = dimension 27 | self.edges = edges 28 | self.weight = weight 29 | self.directions = Direction2D() if dimension == 2 else Direction3D() 30 | 31 | def get_dict_tile(self): 32 | return self.name, self.edges, self.weight 33 | 34 | def get_flipped_tile(self, direction): 35 | if direction not in ["x", "y", "z"]: 36 | raise ValueError(f"Direction {direction} is not defined.") 37 | new_name = f"{self.name}_{direction}" 38 | new_edges = {} 39 | for key, value in self.edges.items(): 40 | new_key = self.directions.flipped_directions[direction][self.directions.base_directions.index(key)] 41 | if key in self.directions.is_edge_flipped[direction]: 42 | new_edges[new_key] = value[::-1] 43 | else: 44 | new_edges[new_key] = value 45 | return Tile(name=new_name, edges=new_edges, dimension=self.dimension, weight=self.weight) 46 | 47 | def get_rotated_tile(self, deg): 48 | if deg not in self.directions.directions: 49 | raise ValueError(f"Rotation degree {deg} is not defined.") 50 | new_name = f"{self.name}_{deg}" 51 | basic_directions = self.directions.directions[0] 52 | new_directions = self.directions.directions[deg] 53 | new_edges = {new_key: self.edges[key] for new_key, key in zip(new_directions, basic_directions)} 54 | return Tile(name=new_name, edges=new_edges, dimension=self.dimension, weight=self.weight) 55 | 56 | def get_all_tiles(self, rotations=(), flips=()): 57 | tiles = [self] 58 | for rotation in rotations: 59 | tiles.append(self.get_rotated_tile(rotation)) 60 | for flip_direction in flips: 61 | tiles.append(self.get_flipped_tile(flip_direction)) 62 | for rotation in rotations: 63 | tiles.append(self.get_flipped_tile(flip_direction).get_rotated_tile(rotation)) 64 | return tiles 65 | 66 | def __str__(self): 67 | return f"Tile {self.name} with edges {self.edges}, weight {self.weight}" 68 | 69 | 70 | class ArrayTile(Tile): 71 | """Class to manage the tiles. 72 | Args: 73 | name (str): Name of the tile. 74 | array (np.ndarray): Array of the tile. 75 | edges (Optional[Dict[str, str]]): Dictionary of the edges of the tile. The keys are the directions and the values are the name of the edge. 76 | Example: 77 | tile = ArrayTile(name="tile", array=np.array([[1, 1, 1], [1, 0, 1], [1, 1, 1]])) 78 | """ 79 | 80 | def __init__( 81 | self, 82 | name: str, 83 | array: np.ndarray, 84 | edges: Optional[Dict[str, str]] = None, 85 | dimension: int = 2, 86 | weight: float = 1.0, 87 | ): 88 | self.array = array 89 | self.directions = Direction2D() 90 | if edges is None: 91 | edges = self.create_edges_from_array(array) 92 | super().__init__(name, edges, dimension, weight) 93 | 94 | def get_array(self, name=None): 95 | if name is None: 96 | return self.array 97 | for deg in (90, 180, 270): 98 | if name == f"{self.name}_{deg}": 99 | a = deg // 90 100 | return np.rot90(self.array, a) 101 | 102 | def get_flipped_tile(self, direction): 103 | # flip array 104 | if direction == "x": 105 | array = np.flip(self.array, 1) 106 | elif direction == "y": 107 | array = np.flip(self.array, 0) 108 | else: 109 | raise ValueError(f"Direction {direction} is not defined.") 110 | tile = super().get_flipped_tile(direction) 111 | return ArrayTile(name=tile.name, array=array, edges=tile.edges, dimension=self.dimension, weight=tile.weight) 112 | 113 | def get_rotated_tile(self, deg): 114 | if deg not in self.directions.directions: 115 | raise ValueError(f"Rotation degree {deg} is not defined.") 116 | a = deg // 90 117 | array = np.rot90(self.array, a) 118 | tile = super().get_rotated_tile(deg) 119 | return ArrayTile(name=tile.name, array=array, edges=tile.edges, dimension=self.dimension, weight=tile.weight) 120 | 121 | def create_edges_from_array(self, array): 122 | """Create a hash for each edge of the tile.""" 123 | edges = {} 124 | for direction in self.directions.base_directions: 125 | if direction == "up": 126 | edges[direction] = tuple(np.round(array[0, :], 1)) 127 | elif direction == "down": 128 | edges[direction] = tuple(np.round(array[-1, :][::-1], 1)) 129 | elif direction == "left": 130 | edges[direction] = tuple(np.round(array[:, 0][::-1], 1)) 131 | elif direction == "right": 132 | edges[direction] = tuple(np.round(array[:, -1], 1)) 133 | else: 134 | raise ValueError(f"Direction {direction} is not defined.") 135 | return edges 136 | 137 | def __str__(self): 138 | return super().__str__() + f"\n {self.array}" 139 | 140 | 141 | class MeshTile(ArrayTile): 142 | def __init__( 143 | self, 144 | name: str, 145 | mesh: Union[trimesh.Trimesh, Callable[[], trimesh.Trimesh]], 146 | array: Optional[np.ndarray] = None, 147 | edges: Optional[Dict[str, str]] = None, 148 | mesh_dim: Tuple[float, float, float] = (2.0, 2.0, 2.0), 149 | array_sample_size: int = 5, 150 | dimension: int = 2, 151 | weight: float = 1.0, 152 | ): 153 | """Class to manage the tiles. 154 | Args: 155 | name: Name of the tile 156 | array: Array of the tile 157 | mesh: Mesh object, or Function to generate the mesh 158 | edges: Edges of the tile 159 | dimension: Dimension of the tile 160 | weight: Weight of the tile 161 | """ 162 | # self.mesh_gen = mesh_gen 163 | self.mesh_gen = lambda: mesh() if callable(mesh) else mesh 164 | if array is None: 165 | array = get_height_array_of_mesh(self.mesh_gen(), mesh_dim, array_sample_size) 166 | super().__init__(name, array, edges, dimension, weight=weight) 167 | 168 | def get_flipped_tile(self, direction): 169 | # flip array 170 | if direction == "x": 171 | mesh_gen = lambda: flip_mesh(self.mesh_gen(), "x") 172 | elif direction == "y": 173 | mesh_gen = lambda: flip_mesh(self.mesh_gen(), "y") 174 | else: 175 | raise ValueError(f"Direction {direction} is not defined.") 176 | # Apply the transformation to the mesh 177 | tile = super().get_flipped_tile(direction) 178 | return MeshTile( 179 | name=tile.name, 180 | array=tile.array, 181 | mesh=mesh_gen, 182 | edges=tile.edges, 183 | dimension=self.dimension, 184 | weight=self.weight, 185 | ) 186 | 187 | def get_rotated_tile(self, deg): 188 | if deg not in self.directions.directions: 189 | raise ValueError(f"Rotation degree {deg} is not defined.") 190 | mesh_gen = lambda: yaw_rotate_mesh(self.mesh_gen(), deg) 191 | tile = super().get_rotated_tile(deg) 192 | return MeshTile( 193 | name=tile.name, 194 | array=tile.array, 195 | mesh=mesh_gen, 196 | edges=tile.edges, 197 | dimension=self.dimension, 198 | weight=self.weight, 199 | ) 200 | 201 | def get_mesh(self): 202 | return self.mesh_gen() 203 | 204 | def __str__(self): 205 | return "MeshTile: " + super().__str__() 206 | --------------------------------------------------------------------------------