diff --git a/examples/worlds/heightmap.sdf b/examples/worlds/heightmap.sdf new file mode 100644 index 0000000000..69f354b513 --- /dev/null +++ b/examples/worlds/heightmap.sdf @@ -0,0 +1,51 @@ + + + + + + + + + + 3D View + false + docked + + + ogre + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -16.6 7.3 8.6 0.0 0.4 -0.45 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + https://fuel.ignitionrobotics.org/1.0/chapulina/models/Heightmap Bowl + + + + diff --git a/src/Conversions.cc b/src/Conversions.cc index e993f4c006..fb46119485 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -48,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -188,6 +190,39 @@ msgs::Geometry ignition::gazebo::convert(const sdf::Geometry &_in) meshMsg->set_submesh(meshSdf->Submesh()); meshMsg->set_center_submesh(meshSdf->CenterSubmesh()); } + else if (_in.Type() == sdf::GeometryType::HEIGHTMAP && _in.HeightmapShape()) + { + auto heightmapSdf = _in.HeightmapShape(); + + out.set_type(msgs::Geometry::HEIGHTMAP); + auto heightmapMsg = out.mutable_heightmap(); + + heightmapMsg->set_filename(asFullPath(heightmapSdf->Uri(), + heightmapSdf->FilePath())); + msgs::Set(heightmapMsg->mutable_size(), heightmapSdf->Size()); + msgs::Set(heightmapMsg->mutable_origin(), heightmapSdf->Position()); + heightmapMsg->set_use_terrain_paging(heightmapSdf->UseTerrainPaging()); + heightmapMsg->set_sampling(heightmapSdf->Sampling()); + + for (auto i = 0u; i < heightmapSdf->TextureCount(); ++i) + { + auto textureSdf = heightmapSdf->TextureByIndex(i); + auto textureMsg = heightmapMsg->add_texture(); + textureMsg->set_size(textureSdf->Size()); + textureMsg->set_diffuse(asFullPath(textureSdf->Diffuse(), + heightmapSdf->FilePath())); + textureMsg->set_normal(asFullPath(textureSdf->Normal(), + heightmapSdf->FilePath())); + } + + for (auto i = 0u; i < heightmapSdf->BlendCount(); ++i) + { + auto blendSdf = heightmapSdf->BlendByIndex(i); + auto blendMsg = heightmapMsg->add_blend(); + blendMsg->set_min_height(blendSdf->MinHeight()); + blendMsg->set_fade_dist(blendSdf->FadeDistance()); + } + } else { ignerr << "Geometry type [" << static_cast(_in.Type()) @@ -252,6 +287,38 @@ sdf::Geometry ignition::gazebo::convert(const msgs::Geometry &_in) out.SetMeshShape(meshShape); } + else if (_in.type() == msgs::Geometry::HEIGHTMAP && _in.has_heightmap()) + { + out.SetType(sdf::GeometryType::HEIGHTMAP); + sdf::Heightmap heightmapShape; + + heightmapShape.SetUri(_in.heightmap().filename()); + heightmapShape.SetSize(msgs::Convert(_in.heightmap().size())); + heightmapShape.SetPosition(msgs::Convert(_in.heightmap().origin())); + heightmapShape.SetUseTerrainPaging(_in.heightmap().use_terrain_paging()); + heightmapShape.SetSampling(_in.heightmap().sampling()); + + for (int i = 0; i < _in.heightmap().texture_size(); ++i) + { + auto textureMsg = _in.heightmap().texture(i); + sdf::HeightmapTexture textureSdf; + textureSdf.SetSize(textureMsg.size()); + textureSdf.SetDiffuse(textureMsg.diffuse()); + textureSdf.SetNormal(textureMsg.normal()); + heightmapShape.AddTexture(textureSdf); + } + + for (int i = 0; i < _in.heightmap().blend_size(); ++i) + { + auto blendMsg = _in.heightmap().blend(i); + sdf::HeightmapBlend blendSdf; + blendSdf.SetMinHeight(blendMsg.min_height()); + blendSdf.SetFadeDistance(blendMsg.fade_dist()); + heightmapShape.AddBlend(blendSdf); + } + + out.SetHeightmapShape(heightmapShape); + } else { ignerr << "Geometry type [" << static_cast(_in.type()) diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 4521697e45..f49bd649d3 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -412,6 +413,72 @@ TEST(Conversions, GeometryPlane) EXPECT_EQ(math::Vector3d::UnitY, newGeometry.PlaneShape()->Normal()); } +///////////////////////////////////////////////// +TEST(Conversions, GeometryHeightmap) +{ + sdf::Geometry geometry; + geometry.SetType(sdf::GeometryType::HEIGHTMAP); + + sdf::Heightmap heightmap; + heightmap.SetUri("file://heights.png"); + heightmap.SetSize(ignition::math::Vector3d(1, 2, 3)); + heightmap.SetPosition(ignition::math::Vector3d(4, 5, 6)); + heightmap.SetUseTerrainPaging(true); + heightmap.SetSampling(16u); + + sdf::HeightmapTexture texture; + texture.SetDiffuse("file://diffuse.png"); + texture.SetNormal("file://normal.png"); + texture.SetSize(1.23); + heightmap.AddTexture(texture); + + sdf::HeightmapBlend blend; + blend.SetMinHeight(123.456); + blend.SetFadeDistance(456.789); + heightmap.AddBlend(blend); + + geometry.SetHeightmapShape(heightmap); + + auto geometryMsg = convert(geometry); + + EXPECT_EQ(msgs::Geometry::HEIGHTMAP, geometryMsg.type()); + EXPECT_TRUE(geometryMsg.has_heightmap()); + EXPECT_EQ(math::Vector3d(1, 2, 3), + msgs::Convert(geometryMsg.heightmap().size())); + EXPECT_EQ("file://heights.png", geometryMsg.heightmap().filename()); + EXPECT_TRUE(geometryMsg.heightmap().use_terrain_paging()); + EXPECT_EQ(16u, geometryMsg.heightmap().sampling()); + + ASSERT_EQ(1, geometryMsg.heightmap().texture().size()); + EXPECT_DOUBLE_EQ(1.23, geometryMsg.heightmap().texture(0).size()); + EXPECT_EQ("file://diffuse.png", geometryMsg.heightmap().texture(0).diffuse()); + EXPECT_EQ("file://normal.png", geometryMsg.heightmap().texture(0).normal()); + + ASSERT_EQ(1, geometryMsg.heightmap().blend().size()); + EXPECT_DOUBLE_EQ(123.456, geometryMsg.heightmap().blend(0).min_height()); + EXPECT_DOUBLE_EQ(456.789, geometryMsg.heightmap().blend(0).fade_dist()); + + auto newGeometry = convert(geometryMsg); + + EXPECT_EQ(sdf::GeometryType::HEIGHTMAP, newGeometry.Type()); + + auto newHeightmap = newGeometry.HeightmapShape(); + ASSERT_NE(nullptr, newHeightmap); + EXPECT_EQ(math::Vector3d(1, 2, 3), newHeightmap->Size()); + EXPECT_EQ("file://heights.png", newHeightmap->Uri()); + EXPECT_TRUE(newHeightmap->UseTerrainPaging()); + EXPECT_EQ(16u, newHeightmap->Sampling()); + + ASSERT_EQ(1u, newHeightmap->TextureCount()); + EXPECT_DOUBLE_EQ(1.23, newHeightmap->TextureByIndex(0)->Size()); + EXPECT_EQ("file://diffuse.png", newHeightmap->TextureByIndex(0)->Diffuse()); + EXPECT_EQ("file://normal.png", newHeightmap->TextureByIndex(0)->Normal()); + + ASSERT_EQ(1u, newHeightmap->BlendCount()); + EXPECT_DOUBLE_EQ(123.456, newHeightmap->BlendByIndex(0)->MinHeight()); + EXPECT_DOUBLE_EQ(456.789, newHeightmap->BlendByIndex(0)->FadeDistance()); +} + ///////////////////////////////////////////////// TEST(Conversions, Inertial) { diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index e8f0c1a0f8..b69a718389 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -27,12 +28,16 @@ #include #include +#include +#include #include #include #include #include #include +#include +#include #include #include #include @@ -283,7 +288,11 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, // set material rendering::MaterialPtr material{nullptr}; - if (_visual.Material()) + if (_visual.Geom()->Type() == sdf::GeometryType::HEIGHTMAP) + { + // Heightmap's material is loaded together with it. + } + else if (_visual.Material()) { material = this->LoadMaterial(*_visual.Material()); } @@ -418,6 +427,57 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, geom = this->dataPtr->scene->CreateMesh(descriptor); scale = _geom.MeshShape()->Scale(); } + else if (_geom.Type() == sdf::GeometryType::HEIGHTMAP) + { + auto fullPath = asFullPath(_geom.HeightmapShape()->Uri(), + _geom.HeightmapShape()->FilePath()); + if (fullPath.empty()) + { + ignerr << "Heightmap geometry missing URI" << std::endl; + return geom; + } + + auto data = std::make_shared(); + if (data->Load(fullPath) < 0) + { + ignerr << "Failed to load heightmap image data from [" << fullPath << "]" + << std::endl; + return geom; + } + + rendering::HeightmapDescriptor descriptor; + descriptor.SetData(data); + descriptor.SetSize(_geom.HeightmapShape()->Size()); + descriptor.SetSampling(_geom.HeightmapShape()->Sampling()); + + for (uint64_t i = 0; i < _geom.HeightmapShape()->TextureCount(); ++i) + { + auto textureSdf = _geom.HeightmapShape()->TextureByIndex(i); + rendering::HeightmapTexture textureDesc; + textureDesc.SetSize(textureSdf->Size()); + textureDesc.SetDiffuse(asFullPath(textureSdf->Diffuse(), + _geom.HeightmapShape()->FilePath())); + textureDesc.SetNormal(asFullPath(textureSdf->Normal(), + _geom.HeightmapShape()->FilePath())); + descriptor.AddTexture(textureDesc); + } + + for (uint64_t i = 0; i < _geom.HeightmapShape()->BlendCount(); ++i) + { + auto blendSdf = _geom.HeightmapShape()->BlendByIndex(i); + rendering::HeightmapBlend blendDesc; + blendDesc.SetMinHeight(blendSdf->MinHeight()); + blendDesc.SetFadeDistance(blendSdf->FadeDistance()); + descriptor.AddBlend(blendDesc); + } + + geom = this->dataPtr->scene->CreateHeightmap(descriptor); + if (nullptr == geom) + { + ignerr << "Failed to create heightmap [" << fullPath << "]" << std::endl; + } + scale = _geom.HeightmapShape()->Size(); + } else { ignerr << "Unsupported geometry type" << std::endl;