Enable ray heightmap ray query (#778)

Ray queries now return intersection points with heightmaps. This fixes the issue with weird view control movements when orbiting / panning with a mouse on heightmaps. However, mouse picking (selecting heightmap visual) is not working yet. It may require a change to API - added a todo for this.

Signed-off-by: Ian Chen <[email protected]>

ogre2/src/Ogre2Heightmap.cc

@@ -234,6 +234,7 @@ void Ogre2Heightmap::Init()
         &ogreSceneManager->_getEntityMemoryManager(
           Ogre::/*SCENE_STATIC*/SCENE_DYNAMIC),
         ogreSceneManager, 11u, ogreCompMgr, nullptr, true );
+
   // Does not cast shadows because it uses a raymarching implementation
   // instead of shadow maps. It does receive shadows from shadow maps though
   this->dataPtr->terra->setCastShadows(false);

ogre2/src/Ogre2RayQuery.cc

@@ -165,22 +165,32 @@ RayQueryResult Ogre2RayQuery::ClosestPointBySelectionBuffer()
       this->dataPtr->imgPos.X(), this->dataPtr->imgPos.Y(), ogreItem, point);
   result.distance = -1;
 
-  if (success && ogreItem)
+  if (success)
   {
-    if (!ogreItem->getUserObjectBindings().getUserAny().isEmpty() &&
-        ogreItem->getUserObjectBindings().getUserAny().getType() ==
-        typeid(unsigned int))
+    double distance = this->dataPtr->camera->WorldPosition().Distance(point)
+        - this->dataPtr->camera->NearClipPlane();
+    unsigned int objectId = 0;
+    if (ogreItem)
     {
-      auto userAny = ogreItem->getUserObjectBindings().getUserAny();
-      double distance = this->dataPtr->camera->WorldPosition().Distance(point)
-          - this->dataPtr->camera->NearClipPlane();
-      if (!std::isinf(distance))
+      if (!ogreItem->getUserObjectBindings().getUserAny().isEmpty() &&
+          ogreItem->getUserObjectBindings().getUserAny().getType() ==
+          typeid(unsigned int))
       {
-        result.distance = distance;
-        result.point = point;
-        result.objectId = Ogre::any_cast<unsigned int>(userAny);
+        auto userAny = ogreItem->getUserObjectBindings().getUserAny();
+        objectId = Ogre::any_cast<unsigned int>(userAny);
       }
     }
+    else
+    {
+      // \todo(anyone) Change ExecuteQuery to return Ogre::MovableObject
+      // in gz-rendering8 so heightmaps can be included in the result
+    }
+    if (!std::isinf(distance))
+    {
+      result.distance = distance;
+      result.point = point;
+      result.objectId = objectId;
+    }
   }
   return result;
 }

ogre2/src/Ogre2SelectionBuffer.cc

@@ -21,6 +21,7 @@
 #include "gz/common/Console.hh"
 #include "gz/rendering/RenderTypes.hh"
 #include "gz/rendering/ogre2/Ogre2Conversions.hh"
+#include "gz/rendering/ogre2/Ogre2Heightmap.hh"
 #include "gz/rendering/ogre2/Ogre2MaterialSwitcher.hh"
 #include "gz/rendering/ogre2/Ogre2RenderEngine.hh"
 #include "gz/rendering/ogre2/Ogre2RenderTarget.hh"
@@ -497,7 +498,29 @@ bool Ogre2SelectionBuffer::ExecuteQuery(const int _x, const int _y,
     auto collection = this->dataPtr->sceneMgr->findMovableObjects(
         Ogre::ItemFactory::FACTORY_TYPE_NAME, entName);
     if (collection.empty())
+    {
+      // try heightmaps
+      auto heightmaps = this->dataPtr->scene->Heightmaps();
+      for (auto h : heightmaps)
+      {
+        auto heightmap = h.lock();
+        if (heightmap)
+        {
+          if (entName == heightmap->Name())
+          {
+            // \todo(anyone) change return type to MovableObject instead of item
+            // in gz-rendering8 so we can uncomment the line below to return a
+            //  heightmap object
+            // _item = std::dynamic_pointer_cast<Ogre2Heightmap>(
+            //     heightmap->OgreObject());
+            _point = point;
+            return true;
+          }
+        }
+      }
       return false;
+
+    }
     else
     {
       _item = dynamic_cast<Ogre::Item *>(collection[0]);

test/common_test/Utils_TEST.cc

@@ -17,9 +17,11 @@
 
 #include "CommonRenderingTest.hh"
 
+#include <gz/common/geospatial/ImageHeightmap.hh>
 #include <gz/utils/ExtraTestMacros.hh>
 
 #include "gz/rendering/Camera.hh"
+#include "gz/rendering/Heightmap.hh"
 #include "gz/rendering/RayQuery.hh"
 #include "gz/rendering/Scene.hh"
 #include "gz/rendering/Utils.hh"
@@ -28,8 +30,12 @@
 using namespace gz;
 using namespace rendering;
 
-class UtilTest : public CommonRenderingTest 
+class UtilTest : public CommonRenderingTest
 {
+  /// \brief Path to test media files.
+  public: const std::string TEST_MEDIA_PATH{
+        common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+        "test", "media")};
 };
 
 /////////////////////////////////////////////////
@@ -39,7 +45,7 @@ TEST_F(UtilTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ClickToScene))
   ASSERT_NE(nullptr, scene);
 
   CameraPtr camera(scene->CreateCamera());
-  EXPECT_NE(nullptr, camera);
+  ASSERT_NE(nullptr, camera);
 
   camera->SetLocalPosition(0.0, 0.0, 15);
   camera->SetLocalRotation(0.0, GZ_PI / 2, 0.0);
@@ -153,3 +159,114 @@ TEST_F(UtilTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ClickToScene))
   // Clean up
   engine->DestroyScene(scene);
 }
+
+/////////////////////////////////////////////////
+TEST_F(UtilTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ClickToSceneHeightmap))
+{
+  CHECK_SUPPORTED_ENGINE("ogre2");
+
+  ScenePtr scene = engine->CreateScene("scene");
+  ASSERT_NE(nullptr, scene);
+
+  CameraPtr camera(scene->CreateCamera());
+  EXPECT_TRUE(camera != nullptr);
+
+  math::Pose3d cameraPose(math::Vector3d(0.0, 0.0, 20.0),
+      math::Quaterniond(0.0, GZ_PI / 2.0, 0.0));
+  camera->SetLocalPosition(cameraPose.Pos());
+  camera->SetLocalRotation(cameraPose.Rot());
+
+  unsigned int width = 640u;
+  unsigned int height = 480u;
+  camera->SetImageWidth(width);
+  camera->SetImageHeight(height);
+
+  // Heightmap data
+  auto heightImage = common::joinPaths(TEST_MEDIA_PATH, "heightmap_bowl.png");
+  math::Vector3d size{100, 100, 10};
+  math::Vector3d position{0, 0, 0};
+  auto textureImage = common::joinPaths(TEST_MEDIA_PATH, "materials",
+      "textures", "texture.png");
+  auto normalImage = common::joinPaths(TEST_MEDIA_PATH, "materials",
+      "textures", "flat_normal.png");
+
+  auto data = std::make_shared<common::ImageHeightmap>();
+  data->Load(heightImage);
+
+  EXPECT_EQ(heightImage, data->Filename());
+
+  HeightmapDescriptor desc;
+  desc.SetData(data);
+  desc.SetSize(size);
+  desc.SetPosition(position);
+  desc.SetUseTerrainPaging(true);
+  desc.SetSampling(4u);
+
+  HeightmapTexture textureA;
+  textureA.SetSize(0.5);
+  textureA.SetDiffuse(textureImage);
+  textureA.SetNormal(normalImage);
+  desc.AddTexture(textureA);
+
+  HeightmapBlend blendA;
+  blendA.SetMinHeight(2.0);
+  blendA.SetFadeDistance(5.0);
+  desc.AddBlend(blendA);
+
+  HeightmapTexture textureB;
+  textureB.SetSize(0.5);
+  textureB.SetDiffuse(textureImage);
+  textureB.SetNormal(normalImage);
+  desc.AddTexture(textureB);
+
+  HeightmapBlend blendB;
+  blendB.SetMinHeight(4.0);
+  blendB.SetFadeDistance(5.0);
+  desc.AddBlend(blendB);
+
+  HeightmapTexture textureC;
+  textureC.SetSize(0.5);
+  textureC.SetDiffuse(textureImage);
+  textureC.SetNormal(normalImage);
+  desc.AddTexture(textureC);
+
+  auto heightmap = scene->CreateHeightmap(desc);
+  ASSERT_NE(nullptr, heightmap);
+
+  // Add to a visual
+  auto vis = scene->CreateVisual();
+  vis->AddGeometry(heightmap);
+  EXPECT_EQ(1u, vis->GeometryCount());
+  EXPECT_TRUE(vis->HasGeometry(heightmap));
+  EXPECT_EQ(heightmap, vis->GeometryByIndex(0));
+  scene->RootVisual()->AddChild(vis);
+
+  // add camera and render one frame
+  scene->RootVisual()->AddChild(camera);
+  camera->Update();
+
+  const int halfWidth  = static_cast<int>(width / 2);
+  const int halfHeight = static_cast<int>(height / 2);
+  math::Vector2i centerClick(halfWidth, halfHeight);
+
+  RayQueryPtr rayQuery = scene->CreateRayQuery();
+  EXPECT_TRUE(rayQuery != nullptr);
+
+  // screenToScene
+  RayQueryResult rayResult;
+  math::Vector3d result =
+      screenToScene(centerClick, camera, rayQuery, rayResult);
+  math::Vector3d expectedPoint(-0.0271169, -0.0271008, 5.00273);
+
+  // Camera should see heightmap point
+  EXPECT_NEAR(expectedPoint.Z(), result.Z(), 4e-6);
+  EXPECT_NEAR(expectedPoint.X(), result.X(), 2e-6);
+  EXPECT_NEAR(expectedPoint.Y(), result.Y(), 2e-6);
+  EXPECT_TRUE(rayResult);
+  EXPECT_NEAR(cameraPose.Pos().Z() - result.Z() - camera->NearClipPlane(),
+      rayResult.distance, 1e-4);
+  EXPECT_EQ(expectedPoint, rayResult.point);
+
+  // Clean up
+  engine->DestroyScene(scene);
+}